public WayPointStruct(int id, Vector3 pos) { _position = pos; _g = 0f; h = 0f; _id = id; _parent = -1; _neigborns = new Neigborns(); }
public void Process() { bool findGoal = false; _map.set(_init, 0.0f, -1, _target); EnqueueOpen(_init); while (OpenSize > 0 && !findGoal) { int current = DequeueOpen(); if (current != _target) //si no es el final... { SetClose(current); Neigborns successors = _map.GetNeighbors(current); for (int i = 0; i < successors.Count; ++i) { int child = successors[i]; float gCostFromThisPath;// = _map.GetCostFromThisPath(current, child); bool inOpen = IsOpen(child); bool isClose = IsClose(child); if (_map.NeedUpdatePath(current, child, inOpen, isClose, out gCostFromThisPath)) { _map.set(child, gCostFromThisPath, current, _target); if (!inOpen) { EnqueueOpen(child); } } } } else { findGoal = true; } Sort(); } if (findGoal) { numWayPoints[0] = ReconstructPath(_target, 0); } }