public void Triggered_Node(Pathnoder node) { if (this.myQueue.Peek() == node) { this.myQueue.Dequeue(); } }
public static Queue <Pathnoder> Pathfind(Pathnoder goal, Pathnoder start) { Associative_Heap <float, Pathnoder> heap = new Associative_Heap <float, Pathnoder>(1); Dictionary <Pathnoder, KeyValuePair <Pathnoder, float> > map = new Dictionary <Pathnoder, KeyValuePair <Pathnoder, float> >(); map[start] = new KeyValuePair <Pathnoder, float>(null, 0f); heap.Add(new KeyValuePair <float, Pathnoder>(0f, start)); while (!heap.IsEmpty) { Pathnoder current = heap.Peak.Value; heap.Pop(); if (goal == current) { Queue <Pathnoder> path = new Queue <Pathnoder>(); path.Enqueue(goal); Pathnoder node = map[goal].Key; for (; node != null;) { path.Enqueue(node); node = map[node].Key; } return(path); } float cost = map[current].Value; foreach (Pathnoder next in current.myEdges.Keys) { float new_cost = cost + current.Cost(next); //if next not in cost_so_far or new_cost < cost_so_far[next]: if (!map.ContainsKey(next) || new_cost < map[next].Value) { // cost_so_far[next] = new_cost // came_from[next] = current map[next] = new KeyValuePair <Pathnoder, float>(current, new_cost); // priority = new_cost + heuristic(goal, next) float priority = new_cost + goal.Cost(next); // frontier.put(next, priority) heap.Add(new KeyValuePair <float, Pathnoder>(priority, next)); } } } return(null); }
float Cost(Pathnoder node) { float distance = (node.transform.position - this.transform.position).magnitude; if (this.myEdges.ContainsKey(node)) { return(this.myEdges[node] * distance); } else { return(distance); } }
void Update() { if (Input.GetKeyDown(KeyCode.Space)) { this.myQueue = Pathnoder.Pathfind(this.myStart, this.myGoal); Debug.Log(this.myQueue); } if (null != this.myQueue && this.myQueue.Count > 0) { Pathnoder node = this.myQueue.Peek(); Vector3 difference = node.transform.position - this.transform.position - this.myRigidbody.velocity; this.myRigidbody.AddForce(difference.normalized * this.mySpeed * Time.deltaTime); } }