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);
}
}
