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);
}
private void _move()
{
if (Path != null)
{
if (_moveCounter < Path.Count)
{
Moving = true;
_updateDestinations();
if (AbleToMove)
{
transform.position = Vector3.MoveTowards(transform.position, Path [_moveCounter].Coordinate + Offset, Time.deltaTime * Speed);
}
if (Vector3.Distance(transform.position, Path [_moveCounter].Coordinate + Offset) < SpillDistance)
{
PreviousNode = Path [_moveCounter];
_moveCounter++;
if (_moveCounter < Path.Count)
{
NextNode = Path [_moveCounter];
}
else
{
FinishedPath();
}
}
}
else
{
Moving = false;
}
}
}
/// <summary>
/// Creates a mutual connection between this node and another node.
/// </summary>
/// <param name="node">the other node.</param>
/// <param name="diagonal">Is the other node diagonally placed from this?</param>
public void SetMutualConnection(qpNode node, bool diagonal = false)
{
if (node != null)
{
if (!Contacts.Contains(node))
{
Contacts.Add(node);
}
if (!node.Contacts.Contains(this))
{
node.Contacts.Add(this);
}
if (!diagonal)
{
if (!NonDiagonalContacts.Contains(node))
{
NonDiagonalContacts.Add(node);
}
if (!node.NonDiagonalContacts.Contains(this))
{
node.NonDiagonalContacts.Add(this);
}
}
}
}
/// <summary>
/// Deregisters a single node
/// </summary>
/// <param name="node"></param>
public void DelistNode(qpNode node)
{
node.outdated = true;
for (int i = node.Contacts.Count; i > 0; i--)
{
node.Contacts [i - 1].RemoveMutualConnection(node);
}
nodes.Remove(node); // shouldn't it also destroy node gameObject?
}
/// <summary>
/// Sets connection to another node.
/// </summary>
/// <param name="node">The other node</param>
public void SetConnection(qpNode node)
{
if (node != null)
{
if (!Contacts.Contains(node))
{
Contacts.Add(node);
}
}
}
/// <summary>
/// Remove a mutual connection between this node and another node.
/// </summary>
/// <param name="otherNode">The other node that this node is currently connected to.</param>
public void RemoveMutualConnection(qpNode otherNode)
{
if (otherNode != null)
{
Contacts.Remove(otherNode);
NonDiagonalContacts.Remove(otherNode);
otherNode.Contacts.Remove(this);
otherNode.NonDiagonalContacts.Add(this);
}
}
/// <summary>
/// Creates a path to the desired node, and begins walking the path
/// </summary>
/// <param name="destination">Desired desination node</param>
public bool MakePath(qpNode destination)
{
List <qpNode> nodes = AStar(_nearNode(), destination);
if (nodes.Count == 0)
{
return(false);
}
SetPath(nodes);
return(true);
}
/// <summary>
/// Update is called once per frame
/// </summary>
protected void FixedUpdate() // Why fixed?..
{
_verifyNodes();
_move();
if (DrawPathInEditor && Path.Count > 0)
{
for (int i = 1; i < Path.Count; i++)
{
qpNode prevNode = Path [i - 1];
Debug.DrawLine(prevNode.Coordinate + Offset, Path [i].Coordinate + 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.Coordinate, node.Coordinate) < radius)
{
retList.Add(node);
}
}
return(retList);
}
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);
}
private qpNode _nearNode()
{
qpNode node = (NextNode == null || !NextNode.outdated ? PreviousNode : NextNode);
if (node != null)
{
if (node.outdated || Vector3.Distance(node.Coordinate, this.transform.position) > (SpillDistance * 1.5))
{
FindClosestNode();
return(PreviousNode);
}
}
return(node);
}
private qpNode _nearNode()
{
qpNode node = (NextNode == null || !NextNode.outdated? PreviousNode : NextNode);
if (node != null)
{
if (node.outdated)
{
FindClosestNode();
return(PreviousNode);
}
}
return(node);
}
/// <summary>
/// Used for A*. Calculates _total - sum of distances between current and end, and current and parent nodes.
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <returns></returns>
public float CalculateTotal(qpNode end)
{
_h = CalculateH(end);
if (_parent != null)
{
_g = CalculateG(_parent) + _parent.G;
}
else
{
_g = 1;
}
_total = _g + _h;
return(_total);
}
/// <summary>
/// Remove a mutual connection between this node and another node.
/// </summary>
/// <param name="otherNode">The other node that this node is currently connected to.</param>
public void RemoveMutualConnection(qpNode otherNode)
{
if (otherNode != null)
{
if (Contacts.Contains(otherNode))
{
Contacts.Remove(otherNode);
}
if (NonDiagonalContacts.Contains(otherNode))
{
NonDiagonalContacts.Remove(otherNode);
}
if (otherNode.Contacts.Contains(this))
{
otherNode.Contacts.Remove(this);
}
if (otherNode.NonDiagonalContacts.Contains(this))
{
otherNode.NonDiagonalContacts.Add(this);
}
}
}
/// <summary>
/// Registers a single node(normally send from qp(qpPointNode)
/// </summary>
/// <param name="selection"></param>
public void RegisterNode(qpNode selection)
{
selection.outdated = false;
nodes.Add(selection);
}
/// <summary>
/// Finds the closest node to the object
/// </summary>
public void FindClosestNode()
{
PreviousNode = qpManager.Instance.FindNodeClosestTo(this.transform.position);
}
/// <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)
{
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); // algorithm 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 built.
{
DestinationNode = end;
complete = true;
bool pathComplete = false;
qpNode node = end;
while (!pathComplete)
{
path.Add(node);
if (node == start)
{
pathComplete = true;
}
node = node.Parent;
}
}
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.Parent = 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.Parent.G;
float math1 = candidate.G;
if (math2 > math1)
{
//candidate is the better parent as it has a lower combined g value.
n.Parent = candidate;
}
}
}
//Calculate h, g and total
if (openList.Count == 0)
{
break;
}
openList.RemoveAt(0);
if (openList.Count == 0)
{
break;
}
//the below 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.Total.CompareTo(node2.Total));
});
candidate = openList [0];
closedList.Add(candidate);
}
Debug.Log("astar completed in " + astarSteps + " steps. Path found:" + complete);
path.Reverse();
return(path);
}
/// <summary>
/// Used for A*. Calculates _g - distance between current and parent nodes.
/// </summary>
public float CalculateG(qpNode parent)
{
return(Vector3.Distance(_coordinate, parent.Coordinate));
}
/// <summary>
/// Used for A*. Calculates _h - distance between current and end nodes.
/// </summary>
public float CalculateH(qpNode end)
{
return(Vector3.Distance(_coordinate, end.Coordinate));
}
public bool CanConnectTo(qpNode candidate)
{
int steps = (int)(Vector3.Distance(candidate.Coordinate, _coordinate) * 2);
float castDistance = Mathf.Abs((candidate.Coordinate - _coordinate).y) + 4;
if (qpManager.Instance.KnownUpDirection == qpGrid.Axis.Z)
{
castDistance = Mathf.Abs((candidate.Coordinate - _coordinate).z) + 4;
}
Vector3 difference = (candidate.Coordinate - _coordinate) / steps;
RaycastHit info;
Vector3 upDirection = qpManager.Instance.UpVector;
Vector3 downDirection = -upDirection;
Vector3 myCoordinate = _coordinate + (upDirection / 5); // ...why?
Vector3 destinationPoint = candidate.Coordinate - difference + (upDirection / 5);
if (Physics.Linecast(myCoordinate, destinationPoint, out info)) // обскурная херня, без которой образуются лишние связи
{
return(false);
}
//Ray cast downward along the previously casted straight line
for (int i = 1; i < steps; i++)
{
int ignoreHits = 0;
RaycastHit[] hits;
Vector3 rayCastPositionStart = new Vector3(myCoordinate.x + (difference.x * i), myCoordinate.y + (castDistance / 2), myCoordinate.z + (difference.z * i));
if (upDirection == new Vector3(0, 0, 1))
{
rayCastPositionStart.y = myCoordinate.y + (difference.y * i);
rayCastPositionStart.z = myCoordinate.z + (castDistance / 2);
}
ScanRayCasts.Add(rayCastPositionStart);
hits = Physics.RaycastAll(rayCastPositionStart, downDirection, castDistance);
if (hits.Length == 0)
{
return(false);
}
List <float> heightPoints = new List <float> ();
foreach (RaycastHit hit in hits)
{
if (qpManager.Instance.disallowedTags.Contains(hit.collider.gameObject.tag))
{
return(false);
}
else if (qpManager.Instance.ignoreTags.Contains(hit.collider.gameObject.tag))
{
ignoreHits++;
}
else
{
if (qpManager.Instance.KnownUpDirection == qpGrid.Axis.Y)
{
heightPoints.Add(hit.point.y);
}
else if (qpManager.Instance.KnownUpDirection == qpGrid.Axis.Z)
{
heightPoints.Add(hit.point.z);
}
}
}
if (hits.Length == ignoreHits)
{
return(false);
}
}
return(true);
}
/// <summary>
/// Used for A*
/// </summary>
public void SetParent(qpNode parent)
{
_parent = parent;
}
/// <summary>
/// Update is called once per frame
/// </summary>
protected void Update()
{
if (Target != null)
{
bool ShouldChase = false;
if (Vector3.Distance(Target.transform.position, this.transform.position) < AgroRadius)
{
Vector3 myOffset = this.transform.position;
Vector3 targetOffset = Target.transform.position;
myOffset.y += .2f;
targetOffset.y += .2f;
if (UseLineOfSight)
{
RaycastHit hit;
if (Physics.Linecast(myOffset, targetOffset, out hit))
{
if (hit.collider.gameObject.tag != this.gameObject.tag && hit.collider.gameObject.tag != Target.gameObject.tag)
{
//Inside agro, but not in Line of Sight
}
else
{
//inside agro, and in LineOFSight-LineCast collides only with ignored tags, making path towards target
ShouldChase = true;
}
}
else if (this.PreviousNode != qpManager.Instance.FindNodeClosestTo(Target.transform.position))
{
//inside agro, and in line of sight, making path towards target
ShouldChase = true;
}
if (DrawLineOfSightLineInEditor)
{
Debug.DrawLine(myOffset, hit.point);
}
}
else
{
//Not using line of sight, and inside agro, making path towards player.
ShouldChase = true;
}
}
if (ShouldChase)
{
if (lastSeenTargetNode != qpManager.Instance.FindNodeClosestTo(Target.transform.position))
{
if (Moving)
{
List <qpNode> prePath = AStar(PreviousNode, lastSeenTargetNode);
for (int i = prePath.Count; i > 0; i--)
{
if (prePath[i - 1] == NextNode)
{
prePath.RemoveRange(0, i - 1);
break;
}
}
SetPath(prePath);
}
else
{
MakePath(Target.transform.position);
}
}
lastSeenTargetNode = qpManager.Instance.FindNodeClosestTo(Target.transform.position);
if (DrawLineOfSightLineInEditor)
{
Debug.DrawLine(this.transform.position, Target.transform.position, Color.magenta, 5f, true);
}
}
}
}
/// <summary>
/// Creates a path to the desired node, and begins walking the path
/// </summary>
/// <param name="destination">Desired desination node</param>
public void MakePath(qpNode destination)
{
SetPath(AStar(_nearNode(), destination));
}
/// <summary>
/// Registers a single node(normally send from qp(qpPointNode)
/// </summary>
/// <param name="selection"></param>
public void RegisterNode(qpNode selection)
{
nodes.Add(selection);
}
/// <summary>
/// Deregisters a single node
/// </summary>
/// <param name="node"></param>
public void DelistNode(qpNode node)
{
node.outdated = true;
nodes.Remove(node);
}