public List <Node> A_Star(Node startNode, Node endNode)
{
/*var startNode = getColsestNodeToPoint(start);
* var endNode = getColsestNodeToPoint(end);*/
// The set of currently discovered nodes that are not evaluated yet.
// Initially, only the start node is known.
var openSet = new List <Node> {
startNode
};
var vistedNodes = new bool[nodes.Count];
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the
// most efficient previous step.
var cameFrom = new Node[nodes.Count];
// For each node, the cost of getting from the start node to that node.
var gScore = new float[nodes.Count];
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
var fScore = new float[nodes.Count];
//initialize all the lists
for (var i = 0; i < nodes.Count; i++)
{
if (endNode is null)
{
continue;
}
cameFrom[i] = null;
vistedNodes[i] = false;
if (!(startNode is null) && nodes[i] == startNode)
{
// The cost of going from start to start is zero.
gScore[i] = 0;
// For the first node, that value is completely heuristic.
fScore[i] = getDistance(startNode.getPosition(), endNode.getPosition());
}
public void Walk(Node target, GameTime gameTime)
{
if (target.getPosition().X > position.X)
{
//rotation.X = 180;
}
else
{
rotation.X = 0;
}
if (Vector3.Distance(new Vector3(this.position.X ,this.position.Y, this.position.Z), target.getPosition()) > position.Y)
{
float dt = (float)gameTime.ElapsedGameTime.TotalMilliseconds / 1000;
this.position.X += (target.getPosition().X - this.position.X) * dt * speed;
this.position.Z += (target.getPosition().Z - this.position.Z) * dt * speed;
}
else
if(Vector3.Distance(new Vector3(this.position.X ,this.position.Y, this.position.Z), target.getPosition()) <= position.Y + 0.1f)
{
this.target = this.target.nextNode;
}
}