Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using UnityEngine;
- using System.Collections;
- using System.Collections.Generic;
- public class Seeker : MonoBehaviour
- {
- public List<NodeElement> path = new List<NodeElement>();
- public NodeElement curNode;
- public NodeElement LastDestination;
- Transform tr;
- public NodeElement LastPathElement()
- {
- return path[path.Count - 1];
- }
- protected virtual void Start()
- {
- tr = transform;
- curNode = PathfindingMap.VecToNode(Mathf.RoundToInt(tr.position.x), Mathf.RoundToInt(tr.position.z));
- path.Add(curNode);
- LastDestination = curNode;
- curNode.occupied = this;
- }
- public Animator anim;
- public float Speed = 4;
- public Quaternion DesiredRotation;
- protected virtual void Rotate()
- {
- if (path.Count > 1)
- {
- DesiredRotation = Quaternion.LookRotation(path[1].pos() - curNode.pos());
- transform.rotation = Quaternion.Slerp(transform.rotation, DesiredRotation, Time.deltaTime * 4);
- }
- }
- public bool StoppedMoving()
- {
- return path.Count <= 1 && searching == false;
- }
- protected virtual void RecalculatePath() { }
- protected virtual void LateUpdate()
- {
- Rotate();
- if (path.Count > 1)
- {
- Vector3 pos = path[1].pos();
- transform.position = Vector3.Lerp(transform.position, pos, Time.deltaTime * Speed);
- if (Vector3.Distance(transform.position, pos) < 0.4f)
- {
- path[0].occupied = null;
- path.RemoveAt(0);
- path[0].occupied = this;
- curNode = path[0];
- }
- anim.SetFloat("Speed", 1);
- }
- else
- {
- if (path.Count == 0)
- {
- path.Add(curNode);
- }
- path[0].occupied = this;
- anim.SetFloat("Speed", 0);
- }
- }
- public void SetDestination(Vector3 _pos, bool recalculating = false)
- {
- int x = Mathf.RoundToInt(_pos.x);
- int y = Mathf.RoundToInt(_pos.z);
- SetDestination(PathfindingMap.VecToNode(x, y), recalculating);
- }
- List<NodeElement> _path = new List<NodeElement>();
- public void SetDestination(NodeElement goal, bool recalculating = false)
- {
- if (goal != null)
- if (SquareGrid.InBounds(goal))
- {
- if (goal != curNode && (goal != LastDestination || recalculating == true) && searching == false)
- {
- LastDestination = goal;
- goal = FindNearestReachable(goal);
- StartCoroutine(MultiThreadAStar(goal));
- }
- }
- }
- protected List<NodeElement> searchedNodes = new List<NodeElement>();
- public NodeElement FindNearestReachable(NodeElement nod)
- {
- searchedNodes = new List<NodeElement>();
- if (nod.Passable(this))
- {
- return nod;
- }
- else
- {
- searchedNodes.Add(nod);
- NodeElement cur = nod;
- int i = 0;
- do
- {
- for (int q = 0; q < SquareGrid.DIRS.Length; q++)
- {
- NodeElement node = PathfindingMap.VecToNode(cur.x + SquareGrid.DIRS[q].x, cur.y + SquareGrid.DIRS[q].y);
- if (!searchedNodes.Contains(node))
- {
- i++;
- searchedNodes.Add(node);
- if (node.Passable(this))
- {
- return node;
- }
- }
- }
- cur = searchedNodes[i];
- }
- while (cur != null);
- return null;
- }
- }
- static public double Heuristic(NodeElement a, NodeElement b)
- {
- return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
- }
- public NodeElement _start, _goal, closestPossible;
- PriorityQueue frontier;
- protected bool searching = false;
- public static int MaxNodesPerSearch = 250;
- public Dictionary<NodeElement, NodeElement> camefrom = new Dictionary<NodeElement, NodeElement>();
- public Dictionary<NodeElement, double> costsofar = new Dictionary<NodeElement, double>();
- protected bool unreachableTarget = false;
- static int count = 0;
- public IEnumerator MultiThreadAStar(NodeElement goal)
- {
- searching = true;
- SquareGrid graph = PathfindingMap.grid();
- frontier = new PriorityQueue();
- frontier.Enqueue(curNode, 0);
- _start = curNode;
- _goal = goal;
- camefrom.Clear();
- costsofar.Clear();
- camefrom[_start] = _start;
- costsofar[_start] = 0;
- closestPossible = _start;
- count = 0;
- while (frontier.Count > 0)
- {
- NodeElement current = frontier.Dequeue();
- if (current.Equals(goal))
- {
- break;
- }
- foreach (var next in SquareGrid.Neighbors(current, this))
- {
- double newCost = costsofar[current] + graph.Cost(current, next);
- if (!costsofar.ContainsKey(next)
- || newCost < costsofar[next])
- {
- count++;
- if (count > MaxNodesPerSearch)
- {
- yield return null;
- count = 0;
- }
- double h = Heuristic(next, goal);
- double h2 = Heuristic(closestPossible, goal);
- double priority = newCost + Heuristic(next, goal);
- frontier.Enqueue(next, priority);
- camefrom[next] = current;
- costsofar[next] = newCost;
- if (h2 > h)
- {
- closestPossible = next;
- }
- }
- }
- }
- FindShortestPath();
- searching = false;
- }
- public void FindShortestPath()
- {
- path.Clear();
- NodeElement next = _goal;
- if (!camefrom.ContainsKey(next))
- {
- next = closestPossible;
- }
- path.Add(next);
- for (int i = 0; i < camefrom.Count; i++)
- {
- next = camefrom[next];
- path.Add(next);
- if (next == _start)
- {
- path.Reverse();
- break;
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement