/// <summary>
/// Deliver the messages
/// </summary>
/// <param name="gtt">The GTT.</param>
protected override void Update(GameTime gtt)
{
gt = gtt;
//primeiro Todas as com tempo 0
while (true)
{
Message m = delayedfila.Peek();
if (m == null)
{
break;
}
if (gtt.TotalGameTime.TotalMilliseconds > m.Timetodeliver)
{
deliver(delayedfila.Pop());
}
else
{
break;
}
}
// o resto respeitando o limite por frame
for (int i = 0; i < numMessagerdeliveredByFrame; i++)
{
if (fila.Count == 0)
{
break;
}
Message m = fila.Pop();
deliver(m);
}
}
public Path FindPath(MapPoint startPoint, MapPoint endPoint, bool diagonal, int movementPoints = (short)-1)
{
var success = false;
var matrix = new PathNode[MapPoint.MapSize + 1];
var openList = new PriorityQueueB <short>(new ComparePfNodeMatrix(matrix));
var closedList = new List <PathNode>();
var location = startPoint.CellId;
var counter = 0;
if (movementPoints == 0)
{
return(Path.GetEmptyPath(CellsInformationProvider.Map, CellsInformationProvider.Map.Cells[startPoint.CellId]));
}
matrix[location].Cell = location;
matrix[location].Parent = -1;
matrix[location].G = 0;
matrix[location].F = EstimateHeuristic;
matrix[location].Status = NodeState.Open;
openList.Push(location);
while (openList.Count > 0)
{
location = openList.Pop();
var locationPoint = new MapPoint(location);
if (matrix[location].Status == NodeState.Closed)
{
continue;
}
if (location == endPoint.CellId)
{
matrix[location].Status = NodeState.Closed;
success = true;
break;
}
if (counter > SearchLimit)
{
return(Path.GetEmptyPath(CellsInformationProvider.Map, CellsInformationProvider.Map.Cells[startPoint.CellId]));
}
for (int i = 0; i < (diagonal ? 8 : 4); i++)
{
var newLocationPoint = locationPoint.GetNearestCellInDirection(Directions[i]);
if (newLocationPoint == null)
{
continue;
}
var newLocation = newLocationPoint.CellId;
if (newLocation < 0 || newLocation >= MapPoint.MapSize)
{
continue;
}
if (!MapPoint.IsInMap(newLocationPoint.X, newLocationPoint.Y))
{
continue;
}
if (!CellsInformationProvider.IsCellWalkable(newLocation))
{
continue;
}
double newG = matrix[location].G + 1;
if ((matrix[newLocation].Status == NodeState.Open ||
matrix[newLocation].Status == NodeState.Closed) &&
matrix[newLocation].G <= newG)
{
continue;
}
matrix[newLocation].Cell = newLocation;
matrix[newLocation].Parent = location;
matrix[newLocation].G = newG;
matrix[newLocation].H = GetHeuristic(newLocationPoint, endPoint);
matrix[newLocation].F = newG + matrix[newLocation].H;
openList.Push(newLocation);
matrix[newLocation].Status = NodeState.Open;
}
counter++;
matrix[location].Status = NodeState.Closed;
}
if (success)
{
var node = matrix[endPoint.CellId];
while (node.Parent != -1)
{
closedList.Add(node);
node = matrix[node.Parent];
}
closedList.Add(node);
}
closedList.Reverse();
if (movementPoints > 0 && closedList.Count + 1 > movementPoints)
{
return(new Path(CellsInformationProvider.Map, closedList.Take(movementPoints + 1).Select(entry => CellsInformationProvider.Map.Cells[entry.Cell])));
}
return(new Path(CellsInformationProvider.Map, closedList.Select(entry => CellsInformationProvider.Map.Cells[entry.Cell])));
}
public MapPoint[] FindReachableCells(MapPoint from, int distance)
{
var result = new List <MapPoint>();
var matrix = new PathNode[MapPoint.MapSize + 1];
var openList = new PriorityQueueB <short>(new ComparePfNodeMatrix(matrix));
var location = from.CellId;
var counter = 0;
if (distance == 0)
{
return new [] { new MapPoint(from.CellId) }
}
;
matrix[location].Cell = location;
matrix[location].Parent = -1;
matrix[location].G = 0;
matrix[location].F = 0;
matrix[location].Status = NodeState.Open;
openList.Push(location);
while (openList.Count > 0)
{
location = openList.Pop();
var locationPoint = new MapPoint(location);
if (matrix[location].Status == NodeState.Closed)
{
continue;
}
if (counter > SearchLimit)
{
break;
}
for (int i = 0; i < 4; i++)
{
var newLocationPoint = locationPoint.GetNearestCellInDirection(Directions[i]);
if (newLocationPoint == null)
{
continue;
}
var newLocation = newLocationPoint.CellId;
if (newLocation < 0 || newLocation >= MapPoint.MapSize)
{
continue;
}
if (!MapPoint.IsInMap(newLocationPoint.X, newLocationPoint.Y))
{
continue;
}
if (!CellsInformationProvider.IsCellWalkable(newLocation))
{
continue;
}
double newG = matrix[location].G + 1;
if ((matrix[newLocation].Status == NodeState.Open ||
matrix[newLocation].Status == NodeState.Closed) &&
matrix[newLocation].G <= newG)
{
continue;
}
matrix[newLocation].Cell = newLocation;
matrix[newLocation].Parent = location;
matrix[newLocation].G = newG;
matrix[newLocation].H = 0;
matrix[newLocation].F = newG + matrix[newLocation].H;
if (newG <= distance)
{
result.Add(newLocationPoint);
openList.Push(newLocation);
matrix[newLocation].Status = NodeState.Open;
}
}
counter++;
matrix[location].Status = NodeState.Closed;
}
return(result.ToArray());
}
public List <Node> Astar(int[,] map, Vector2Int startPos, Vector2Int goalPos)
{
bool found = false;
bool stop = false;
PriorityQueueB <Node> open = new PriorityQueueB <Node>(new ComparePFNode());
List <Node> close = new List <Node>();
Node parentNode = new Node(startPos);
Node foundNode = null;
parentNode.GCost = 0;
parentNode.HCost = 0;
parentNode.FCost = parentNode.GCost + parentNode.HCost;
open.Push(parentNode);
while (open.Count > 0 && !stop)
{
parentNode = open.Pop();
if (parentNode.Position.x == goalPos.x && parentNode.Position.y == goalPos.y)
{
foundNode = parentNode;
close.Add(parentNode);
found = true;
break;
}
if (close.Count > SearchLimit)
{
Debug.LogWarning("Search limit exceeded.");
return(null);
}
var successors = GetAllSuccessorNodes(map, parentNode);
//Lets calculate each successors
foreach (var node in successors)
{
float newG = parentNode.GCost + 0; // 0 is tile difficulty
//if ((int)newG == (int)parentNode.GCost)
//{
// Debug.Log("WHAT?");
// continue;
//}
// Need a proper search method
int foundInOpenIndex = -1;
for (int j = 0; j < open.Count; j++)
{
if (open[j].Position.x == node.Position.x && open[j].Position.y == node.Position.y)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && open[foundInOpenIndex].GCost <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < close.Count; j++)
{
if (close[j].Position.x == node.Position.x && close[j].Position.y == node.Position.y)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && close[foundInCloseIndex].GCost <= newG)
{
continue;
}
node.Parent = parentNode;
node.GCost = newG;
node.HCost = Vector2Int.Distance(node.Position, goalPos);
node.FCost = node.GCost + node.HCost;
open.Push(node);
}
close.Add(parentNode);
}
if (found)
{
List <Node> path = new List <Node>();
ConstructPathRecursive(foundNode, path);
path.Reverse();
return(path);
}
return(null);
}
public List <Point> CalculatePath(Point start, Point end, int characterWidth, int characterHeight, short maxJumpHeight)
{
lock (this) //locked to prevent multiple algorithms from running at the same time
{
//clear the lists at the previously touched locations
while (TouchedLocations.Count > 0)
{
nodes[TouchedLocations.Pop()].Clear();
}
//check if the bottom right of the character will be able to fit in the goal
bool inSolidTile = false;
for (var i = 0; i < characterWidth; ++i)
{
inSolidTile = false;
//check characterWidth number of nodes to the right of the goal
for (var w = 0; w < characterWidth; ++w)
{
if (Grid[end.X + w, end.Y] == 0 || Grid[end.X + w, end.Y - characterHeight + 1] == 0)
{
inSolidTile = true;
break;
}
}
if (inSolidTile == false)
{
//check characterHeight number of blocks
for (var h = 0; h < characterHeight; ++h)
{
if (Grid[end.X, end.Y - h] == 0 || Grid[end.X + characterWidth - 1, end.Y - h] == 0)
{
inSolidTile = true;
break;
}
}
}
if (inSolidTile)
{
end.X -= characterWidth - 1;
}
else
{
break;
}
}
if (inSolidTile == true)
{
Main.NewText("Character cannot fit in end location, exiting...");
return(null);
}
Found = false;
Stop = false;
mStopped = false;
CloseNodeCounter = 0;
OpenNodeValue += 2;
CloseNodeValue += 2;
Open.Clear();
Location.xy = (start.Y << GridXLog2) + start.X; //find starting node location in grid
Location.z = 0;
EndLocation = (end.Y << GridXLog2) + end.X; //do the same for the end location
Node firstNode = new Node(); //this is the start node
firstNode.G = 0;
firstNode.F = mHEstimate;
firstNode.PX = (ushort)start.X;
firstNode.PY = (ushort)start.Y;
firstNode.PZ = 0;
firstNode.Status = OpenNodeValue;
//check all nodes beneath the character to see if they are on the ground
bool startsOnGround = false;
for (int x = start.X; x < start.X + characterWidth; ++x)
{
if (Map.IsGround(x, start.Y + 1))
{
startsOnGround = true;
break;
}
}
if (startsOnGround)
{
firstNode.JumpLength = 0;
}
else
{
firstNode.JumpLength = (short)(maxJumpHeight * 2);
}
nodes[Location.xy].Add(firstNode);
TouchedLocations.Push(Location.xy);
Open.Push(Location); //push the starting node into the open set
//the actual algorithm starts here
while (Open.Count > 0 && !Stop)
{
Location = Open.Pop();
//is it in closed list? means this node was already processed and skip this iteration
if (nodes[Location.xy][Location.z].Status == CloseNodeValue)
{
continue;
}
//calculate node we are evaulating
LocationX = (ushort)(Location.xy & GridXMinus1);
LocationY = (ushort)(Location.xy >> GridXLog2);
//current node is the end location
if (Location.xy == EndLocation)
{
nodes[Location.xy][Location.z] = nodes[Location.xy][Location.z].UpdateStatus(CloseNodeValue);
Found = true;
break;
}
//closed nodes has reach threshold, either no path possible or just too many darn nodes to sift through
if (CloseNodeCounter > mSearchLimit)
{
Main.NewText("Hit maximum search limit threshold, exiting...");
mStopped = true;
return(null);
}
//calculate the nodes around the current one
for (var i = 0; i < (mDiagonals ? 8 : 4); i++)
{
NewLocationX = (ushort)(LocationX + mDirection[i, 0]);
NewLocationY = (ushort)(LocationY + mDirection[i, 1]);
NewLocation = (NewLocationY << GridXLog2) + NewLocationX;
var onGround = false;
var atCeiling = false;
for (var w = 0; w < characterWidth; ++w)
{
//check if top most and bottom most blocks of the character to see if they are a solid block
if (Grid[NewLocationX + w, NewLocationY] == 0 || Grid[NewLocationX + w, NewLocationY - characterHeight + 1] == 0)
{
goto CHILDREN_LOOP_END;
}
//any of the bottom nodes right above the ground (air block between them) then it is onGround
if (Map.IsGround(NewLocationX + w, NewLocationY + 1))
{
onGround = true;
}
else if (Grid[NewLocationX + w, NewLocationY - characterHeight] == 0) //any tiles above the character are solid then character would be at ceiling
{
atCeiling = true;
}
}
//check the left and right cells of the character, skip if they are blocks b/c character won't fit in that position
for (var h = 1; h < characterHeight - 1; ++h)
{
if (Grid[NewLocationX, NewLocationY - h] == 0 || Grid[NewLocationX + characterHeight - 1, NewLocationY - h] == 0)
{
goto CHILDREN_LOOP_END;
}
}
//calculate jumplength value for neighbor node
var jumpLength = nodes[Location.xy][Location.z].JumpLength;
short newJumpLength = jumpLength;
if (atCeiling)
{
if (NewLocationX != LocationX)
{
//character needs to drop straight down
//we are falling and our next move needs to be done vertically
newJumpLength = (short)Math.Max(maxJumpHeight * 2 + 1, jumpLength + 1);
}
else
{
//character can still move one cell to either side
//since value is even, the neighbor node will still be able to move either left or right
newJumpLength = (short)Math.Max(maxJumpHeight * 2, jumpLength + 2);
}
}
else if (onGround)
{
newJumpLength = 0;
}
//calcualting jump value mid-air
else if (NewLocationY < LocationY) // neighbor node is above parent node
{
if (jumpLength < 2) //first jump is always two blocks up instead of one up and optionally one to either right or left
{
newJumpLength = 3;
}
else if (jumpLength % 2 == 0)
{
//jump length is even, increment by 2 otherwise increment by 1
//this is to handle jumps that are straight up
newJumpLength = (short)(jumpLength + 2);
}
else
{
newJumpLength = (short)(jumpLength + 1);
}
}
else if (NewLocationY > LocationY) //neighbor node is below the parent
{
//same calculation for vertical jumps, just downwards
if (jumpLength % 2 == 0)
{
newJumpLength = (short)Math.Max(maxJumpHeight * 2, jumpLength + 2);
}
else
{
newJumpLength = (short)Math.Max(maxJumpHeight * 2, jumpLength + 1);
}
}
else if (!onGround && NewLocationX != LocationX) //node is to the left or right of parent node, same y-axis
{
newJumpLength = (short)(jumpLength + 1);
}
//dismiss this node if it's jump value is odd and the parent is either to the left/right
//character went to side already and needs to move up/down
if (jumpLength >= 0 && jumpLength % 2 != 0 && LocationX != NewLocationX)
{
continue;
}
//if we're falling and neighbor node is higher, skip impossible jump
if (jumpLength >= maxJumpHeight * 2 && NewLocationY < LocationY)
{
continue;
}
//prevent giving incorrect values when the character is falling really fast
//without this, character would be moving 1 block to side and 2 or more blocks down instead of 1 block to side and 1 block down
if (newJumpLength >= maxJumpHeight * 2 + 6 && NewLocationX != LocationX && (newJumpLength - (maxJumpHeight * 2 + 6)) % 8 != 3)
{
continue;
}
//cost higher if jump value higher
//divide by 4 to make character stick to ground more often and not get 2 jumpy
NewG = nodes[Location.xy][Location.z].G + Grid[NewLocationX, NewLocationY] + newJumpLength / 4;
//revisit nodes with different jump values
if (nodes[NewLocation].Count > 0)
{
int lowestJump = short.MaxValue;
bool couldMoveSideways = false;
for (int j = 0; j < nodes[NewLocation].Count; ++j)
{
if (nodes[NewLocation][j].JumpLength < lowestJump)
{
lowestJump = nodes[NewLocation][j].JumpLength;
}
if (nodes[NewLocation][j].JumpLength % 2 == 0 && nodes[NewLocation][j].JumpLength < maxJumpHeight * 2 + 6)
{
couldMoveSideways = true;
}
}
//skip node if jump value isn't lower than any of the other nodes at the same x, y (doesn't promise higher jump)
//and the currently processed node's jump value is even and all the others aren't (node allows for sideways movement while others only allow up or down)
if (lowestJump <= newJumpLength && (newJumpLength % 2 != 0 || newJumpLength >= maxJumpHeight * 2 + 6 || couldMoveSideways))
{
continue;
}
}
//calculate H (heuristic) cost
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
H = mHEstimate * (Math.Abs(NewLocationX - end.X) + Math.Abs(NewLocationY - end.Y));
break;
case HeuristicFormula.MaxDXDY:
H = mHEstimate * (Math.Max(Math.Abs(NewLocationX - end.X), Math.Abs(NewLocationY - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
var h_diagonal = Math.Min(Math.Abs(NewLocationX - end.X), Math.Abs(NewLocationY - end.Y));
var h_straight = (Math.Abs(NewLocationX - end.X) + Math.Abs(NewLocationY - end.Y));
H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
H = (int)(mHEstimate * Math.Sqrt(Math.Pow(NewLocationY - end.X, 2) + Math.Pow(NewLocationY - end.Y, 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
H = (int)(mHEstimate * (Math.Pow(NewLocationY - end.X, 2) + Math.Pow(NewLocationY - end.Y, 2)));
break;
case HeuristicFormula.Custom1:
var dxy = new Point(Math.Abs(end.X - NewLocationX), Math.Abs(end.Y - NewLocationY));
var orthogonal = Math.Abs(dxy.X - dxy.Y);
var diagonal = Math.Abs((dxy.X + dxy.Y - orthogonal) / 2);
H = mHEstimate * (diagonal + orthogonal + dxy.X + dxy.Y);
break;
}
//add node to the node list after checks and calculations, phew...
Node newNode = new Node();
newNode.JumpLength = newJumpLength;
newNode.PX = LocationX;
newNode.PY = LocationY;
newNode.PZ = (byte)Location.z;
newNode.G = NewG;
newNode.F = NewG + H;
newNode.Status = OpenNodeValue;
if (nodes[NewLocation].Count == 0)
{
TouchedLocations.Push(NewLocation);
}
nodes[NewLocation].Add(newNode);
Open.Push(new Location(NewLocation, nodes[NewLocation].Count - 1));
//removes the need to break loop and continue to next node
CHILDREN_LOOP_END:
continue;
}
//set status of parent node to "closed"
nodes[Location.xy][Location.z] = nodes[Location.xy][Location.z].UpdateStatus(CloseNodeValue);
CloseNodeCounter++;
}
//filter only necessary nodes
if (Found)
{
//start at the end
Close.Clear();
int posX = end.X;
int posY = end.Y;
Node fPrevNodeTmp = new Node();
Node fNodeTmp = nodes[EndLocation][0];
Point fNode = end;
Point fPrevNode = end;
//points to the next node to be evaluated
var loc = (fNodeTmp.PY << GridXLog2) + fNodeTmp.PX;
//keep going until parent's position == node's position (we hit the start node)
while (fNode.X != fNodeTmp.PX || fNode.Y != fNodeTmp.PY)
{
Node fNextNodeTmp = nodes[loc][fNodeTmp.PZ];
if (Close.Count == 0 || //add end node
Map.IsOneWayPlatform(fNode.X, fNode.Y + 1) ||
(Grid[fNode.X, fNode.Y + 1] == 0 && Map.IsOneWayPlatform(fPrevNode.X, fPrevNode.Y + 1)) ||
fNodeTmp.JumpLength == 3 || //add first jump up or first in air direction change
(fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0) || //add landing node (node that has non-zero jump value becomes 0)
(fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0) //next node is on ground while next one isn't (landing node)
//node y-coordinate is higher than previous and next node in closed list (highest point of jump)
|| (fNode.Y > Close[Close.Count - 1].Y && fNode.Y > fNodeTmp.PY) ||
(fNode.Y < Close[Close.Count - 1].Y && fNode.Y < fNodeTmp.PY)
//next to an obstacle and previous node isn't aligned with current one either horizontally or vertically (went around an obstacle)
|| ((Map.IsGround(fNode.X - 1, fNode.Y) || Map.IsGround(fNode.X + 1, fNode.Y)) &&
fNode.Y != Close[Close.Count - 1].Y && fNode.X != Close[Close.Count - 1].X))
{
Close.Add(fNode);
}
fPrevNode = fNode;
posX = fNodeTmp.PX;
posY = fNodeTmp.PY;
fPrevNodeTmp = fNodeTmp;
fNodeTmp = fNextNodeTmp;
loc = (fNodeTmp.PY << GridXLog2) + fNodeTmp.PX;
fNode = new Point(posX, posY);
}
Close.Add(fNode); //at start of list which means fNode = start node
mStopped = true;
return(Close);
}
Main.NewText("No path found, exiting...");
mStopped = true;
return(null);
}
}
public virtual void Iterate()
{
if (found)
{
return;
}
//for (int iteration = 0; iteration < 100; iteration++)
{
while (openQueue.Count > 0 && !forceStop)
{
parentNode = openQueue.Pop();
if (parentNode.X == endNode.X && parentNode.Y == endNode.Y)
{
closeList.Add(parentNode);
found = true;
break;
}
for (int index = 0; index < 8; index++)
{
int newX = parentNode.X + direction[index, 0];
int newY = parentNode.Y + direction[index, 1];
if (newX < 0 || newY < 0 || newX >= map.Width || newY >= map.Height)
{
// probing out of map
continue;
}
AIPathfinderNode newNode = new AIPathfinderNode();
newNode.X = newX;
newNode.Y = newY;
newNode.G = (map.Node(newNode.X, newNode.Y)).Type;
int newG = parentNode.G + grid[newNode.X, newNode.Y];
if (newG == parentNode.G)
{
continue;
}
int foundInOpenIndex = -1;
for (int j = 0; j < openQueue.Count; j++)
{
if (openQueue[j].X == newNode.X && openQueue[j].Y == newNode.Y)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && openQueue[foundInOpenIndex].G <= newG)
{
continue;
}
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.G = newG;
newNode.H = heuristicEstimateValue * (
Math.Abs(newNode.X - endNode.X) +
Math.Abs(newNode.Y - endNode.Y));
newNode.F = newNode.G + newNode.H;
openQueue.Push(newNode);
}
closeList.Add(parentNode);
return;
//break;
}
}
if (found)
{
AIPathfinderNode fNode = closeList[closeList.Count - 1];
for (int i = closeList.Count - 1; i >= 0; i--)
{
if (fNode.PX == closeList[i].X && fNode.PY == closeList[i].Y || i == closeList.Count - 1)
{
fNode = closeList[i];
}
else
{
closeList.RemoveAt(i);
}
}
this.stopped = true;
this.state = AIPathfinderState.Finished;
return;
//return mClose;
}
stopped = true;
this.state = AIPathfinderState.Failed;
return;
}
/// <summary>
/// Compute and return the path
/// </summary>
/// <param name="Start">The start.</param>
/// <param name="End">The end.</param>
/// <param name="iterations">The iterations.</param>
/// <returns></returns>
public LinkedList <Waypoint> GetPath(Waypoint Start, Waypoint End, float iterations = float.MaxValue)
{
parentNode = Start;
parentNode.Node.parentId = parentNode.Id;
parentNode.Node.G = 0;
parentNode.Node.H = (int)(heuristicEstimateValue * (
Math.Abs(Start.WorldPos.X - End.WorldPos.X) +
Math.Abs(Start.WorldPos.Y - End.WorldPos.Y) +
Math.Abs(Start.WorldPos.Z - End.WorldPos.Z))
);
parentNode.Node.F = parentNode.Node.G + parentNode.Node.H;
parentNode.Node.PX = parentNode.WorldPos.X;
parentNode.Node.PY = parentNode.WorldPos.Y;
parentNode.Node.PZ = parentNode.WorldPos.Z;
openQueue.Clear();
closeList.Clear();
openQueue.Push(parentNode);
bool found = false;
int iter = 0;
while (openQueue.Count > 0 || iter < iterations)
{
iter++;
parentNode = openQueue.Pop();
if (parentNode.Id == End.Id)
{
closeList.Add(parentNode);
found = true;
break;
}
foreach (int var in parentNode.NeightBorWaypointsId)
{
Waypoint way = map.Waypoints.IdWaypoint[var];
if (closeList.Contains(way))
{
continue;
}
float costToCross;
if (CostToCross != null)
{
costToCross = CostToCross(way.WayType);
}
else
{
costToCross = CostToCrossImplementation(way.WayType);
}
float newG = parentNode.Node.G + Vector3.Distance(way.WorldPos, parentNode.WorldPos) * costToCross;
if (newG == parentNode.Node.G)
{
continue;
}
int foundInOpenIndex = -1;
for (int j = 0; j < openQueue.Count; j++)
{
//ve se ele ta na open list
if (openQueue[j].WorldPos.Equals(way.WorldPos))
{
foundInOpenIndex = j;
break;
}
}
//se tiver na openlist e o custo pelo atual caminho for maior entao descarta
if (foundInOpenIndex != -1 && openQueue[foundInOpenIndex].Node.G <= newG)
{
continue;
}
way.Node.PX = parentNode.WorldPos.X;
way.Node.PY = parentNode.WorldPos.Y;
way.Node.PZ = parentNode.WorldPos.Z;
way.Node.G = newG;
way.Node.parentId = parentNode.Id;
way.Node.H = heuristicEstimateValue * (
Math.Abs(way.WorldPos.X - End.WorldPos.X) +
Math.Abs(way.WorldPos.Y - End.WorldPos.Y) +
Math.Abs(way.WorldPos.Z - End.WorldPos.Z)
);
way.Node.F = way.Node.G + way.Node.H;
openQueue.Push(way);
}
closeList.Add(parentNode);
continue;
//break;
}
if (found)
{
ended = true;
Waypoint fNode = closeList[closeList.Count - 1]; //objetivo
//limpa a lista
for (int i = closeList.Count - 1; i >= 0; i--)
{
if ((fNode.Node.PZ == closeList[i].Node.Z && fNode.Node.PX == closeList[i].Node.X && fNode.Node.PY == closeList[i].Node.Y) || i == closeList.Count - 1)
{
fNode = closeList[i];
}
else
{
closeList.RemoveAt(i);
}
}
Waypoint ww;
LinkedList <Waypoint> w = new LinkedList <Waypoint>();
w.AddFirst(End);
ww = map.Waypoints.IdWaypoint[End.Node.parentId];
while (true)
{
if (ww.Equals(Start))
{
w.AddFirst(ww);
break;
}
else
{
w.AddFirst(ww);
ww = map.Waypoints.IdWaypoint[ww.Node.parentId];
}
}
return(w);
}
else if (iterations != float.MaxValue)
{
ended = false;
Waypoint fNode = closeList[closeList.Count - 1]; //objetivo
Waypoint ww;
LinkedList <Waypoint> w = new LinkedList <Waypoint>();
w.AddFirst(End);
ww = map.Waypoints.IdWaypoint[End.Node.parentId];
while (true)
{
if (ww.Equals(Start))
{
w.AddFirst(ww);
break;
}
else
{
w.AddFirst(ww);
ww = map.Waypoints.IdWaypoint[ww.Node.parentId];
}
}
return(w);
}
else
{
ended = false;
return(null);
}
}
protected void ProcessCallback(object state)
{
if (!IsRunning)
{
return;
}
if (Interlocked.CompareExchange(ref m_currentThreadId, Thread.CurrentThread.ManagedThreadId, 0) != 0)
{
return;
}
long timerStart = 0;
// get the time at the start of our task processing
timerStart = m_queueTimer.ElapsedMilliseconds;
var updateDt = (int)(timerStart - m_lastUpdate);
m_lastUpdate = (int)timerStart;
int msgCount = 0;
int timersCount = 0;
var list = new List <IMessage>();
try
{
// process messages
IMessage msg;
while (m_messageQueue.TryDequeue(out msg))
{
msgCount++;
list.Add(msg);
try
{
msg.Execute();
}
catch (Exception ex)
{
logger.Error("Failed to execute message {0} : {1}", msg, ex);
}
if (!IsRunning)
{
return;
}
}
foreach (var timer in m_pausedTimers.Where(timer => timer.Enabled))
{
m_timers.Push(timer);
}
TimedTimerEntry peek;
while ((peek = m_timers.Peek()) != null && peek.NextTick <= DateTime.Now)
{
timersCount++;
var timer = m_timers.Pop();
if (!timer.Enabled)
{
if (!timer.IsDisposed)
{
m_pausedTimers.Add(timer);
}
}
else
{
try
{
timer.Trigger();
if (timer.Enabled)
{
m_timers.Push(timer);
}
}
catch (Exception ex)
{
logger.Error("Exception raised when processing TimerEntry {2} in {0} : {1}.", this, ex, timer);
}
}
}
}
catch (Exception ex)
{
logger.Error("Failed to run TaskQueue callback for \"{0}\" : {1}", Name, ex);
}
finally
{
// get the end time
var timerStop = m_queueTimer.ElapsedMilliseconds;
var updateLagged = timerStop - timerStart > UpdateInterval;
var callbackTimeout = updateLagged ? 0 : ((timerStart + UpdateInterval) - timerStop);
Interlocked.Exchange(ref m_currentThreadId, 0);
if (updateLagged)
{
#if DEBUG
logger.Debug("TaskPool '{0}' update lagged ({1}ms) (msg:{2}, timers:{3}/{4})", Name, timerStop - timerStart, msgCount, timersCount, m_timers.Count);
var orderList = list.OrderByDescending(x =>
{
try
{
return(((dynamic)x).ElapsedTime);
}
catch
{
return(0);
}
}).ToList();
for (int i = 0; i < 10 && i < orderList.Count; i++)
{
logger.Debug("{0}", orderList[i]);
}
#endif
}
if (IsRunning)
{
// re-register the Update-callback
m_updateTask = Task.Factory.StartNewDelayed((int)callbackTimeout, ProcessCallback, this);
}
else
{
m_stoppedAsync.Set();
}
}
}
public bool Rebuild(IEnumerable<Point> blockers)
{
NavNode[] newGrid = new NavNode[_map.Width * _map.Height];
PriorityQueueB<int> open = new PriorityQueueB<int>( (a,b) => newGrid[a]._cost.CompareTo(newGrid[b]._cost) );
open.Push(_targetX + _targetY * _map.Width);
for(int i = 0; i < newGrid.Length; ++i)
{
newGrid[i]._cost = _map.Blocks[i].Type == BlockType.Solid ? BlockerCost : 0;
newGrid[i]._nextIndex = -1;
}
if(blockers != null)
{
foreach(Point p in blockers)
{
newGrid[p.X + p.Y * _map.Width]._cost = BlockerCost;
}
}
while(open.Count > 0)
{
// Get the lowest cost open node
int nodeIndex = open.Pop();
int nodeX = nodeIndex % _map.Width;
int nodeY = nodeIndex / _map.Width;
for(int i = 0; i < 8; ++i)
{
int nextNodeX = nodeX + NeighborLookups[i*2+0];
int nextNodeY = nodeY + NeighborLookups[i*2+1];
if(nextNodeX < 0 || nextNodeX >= _map.Width || nextNodeY < 0 || nextNodeY >= _map.Height)
continue;
int nextNodeIndex = nextNodeX + nextNodeY * _map.Width;
int nextCost = newGrid[nodeIndex]._cost + (i >= 4 ? 14 : 10);
if(IsValidMove(newGrid, nodeX, nodeY, nextNodeX, nextNodeY))
{
// If the neighbor is not already visited, and walkable
if(newGrid[nextNodeIndex]._nextIndex == -1)
{
newGrid[nextNodeIndex]._cost = nextCost;
newGrid[nextNodeIndex]._nextIndex = nodeIndex;
// add it to the open queue
open.Push(nextNodeIndex);
}
else // Update the neighbor if this is a shorter path
if(nextCost < newGrid[nextNodeIndex]._cost)
{
newGrid[nextNodeIndex]._cost = nextCost;
newGrid[nextNodeIndex]._nextIndex = nodeIndex;
}
}
}
}
// Verify that all enemy spawn points can reach the target.
if( 0 != _map.EnemySpawns.Count(sp => newGrid[sp.BlockX + sp.BlockY * _map.Width]._nextIndex < 0))
{
return false;
}
Grid = newGrid;
return true;
}
private void UpdateCallback(object state)
{
if ((IsDisposed || !IsRunning) ||
(Interlocked.CompareExchange(ref m_currentThreadId, Thread.CurrentThread.ManagedThreadId, 0) != 0))
{
logger.Info($"Area {this} exit callback since it's disposed");
return;
}
var updateStart = DateTime.Now;
var updateDelta = (int)((updateStart - m_lastUpdateTime).TotalMilliseconds);
long messageProcessTime = 0;
long timerProcessingTime = 0;
var timerProcessed = 0;
var processedMessages = new List <BenchmarkEntry>();
try
{
var sw = Stopwatch.StartNew();
IMessage msg;
while (m_messageQueue.TryDequeue(out msg))
{
var swMsg = Stopwatch.StartNew();
try
{
msg.Execute();
swMsg.Stop();
if (BenchmarkManager.Enable && swMsg.Elapsed.TotalMilliseconds > 50)
{
processedMessages.Add(BenchmarkEntry.Create(msg.ToString(), swMsg.Elapsed, "area", Id));
}
}
catch (Exception ex)
{
swMsg.Stop();
logger.Error("Exception raised when processing Message in {0} : {1}.", this, ex);
if (BenchmarkManager.Enable)
{
processedMessages.Add(BenchmarkEntry.Create(msg.ToString(), swMsg.Elapsed, "area", Id, "exception", ex));
}
}
}
sw.Stop();
messageProcessTime = sw.ElapsedMilliseconds;
m_isUpdating = true;
foreach (var timer in m_pausedTimers.Where(timer => timer.Enabled))
{
m_timers.Push(timer);
}
sw = Stopwatch.StartNew();
TimedTimerEntry peek;
while ((peek = m_timers.Peek()) != null && peek.NextTick <= DateTime.Now)
{
var timer = m_timers.Pop();
if (!timer.Enabled)
{
if (!timer.IsDisposed)
{
m_pausedTimers.Add(timer);
}
}
else
{
try
{
var swMsg = Stopwatch.StartNew();
timer.Trigger();
swMsg.Stop();
if (BenchmarkManager.Enable && swMsg.Elapsed.TotalMilliseconds > 20)
{
processedMessages.Add(BenchmarkEntry.Create(timer.ToString(), swMsg.Elapsed, "area", Id));
}
if (timer.Enabled)
{
m_timers.Push(timer);
}
timerProcessed++;
}
catch (Exception ex)
{
logger.Error("Exception raised when processing TimerEntry in {0} : {1}.", this, ex);
}
}
}
sw.Stop();
timerProcessingTime = sw.ElapsedMilliseconds;
}
finally
{
try
{
// we updated the map, so set our last update time to now
m_lastUpdateTime = updateStart;
TickCount++;
m_isUpdating = false;
// get the time, now that we've finished our update callback
var updateEnd = DateTime.Now;
var newUpdateDelta = updateEnd - updateStart;
// weigh old update-time 9 times and new update-time once
AverageUpdateTime = ((AverageUpdateTime * 9) + (float)(newUpdateDelta).TotalMilliseconds) / 10;
// make sure to unset the ID *before* enqueuing the task in the ThreadPool again
Interlocked.Exchange(ref m_currentThreadId, 0);
var callbackTimeout = (int)(m_updateDelay - newUpdateDelta.TotalMilliseconds);
if (callbackTimeout < 0)
{
// even if we are in a hurry: For the sake of load-balance we have to give control back to the ThreadPool
callbackTimeout = 0;
logger.Debug("Area '{0}' update lagged ({1}ms) (msg:{2}ms, timers:{3}ms, timerProc:{4}/{5})",
this, (int)newUpdateDelta.TotalMilliseconds, messageProcessTime, timerProcessingTime, timerProcessed, m_timers.Count);
foreach (var msg in processedMessages.OrderByDescending(x => x.Timestamp).Take(15))
{
logger.Debug(msg);
}
BenchmarkManager.Instance.AddRange(processedMessages.OrderByDescending(x => x.Timestamp).Take(15));
}
if (!m_running)
{
m_stoppedAsync.Set();
}
else
{
m_currentTask = Task.Factory.StartNewDelayed(callbackTimeout, UpdateCallback, this);
}
}
catch (Exception ex)
{
logger.Error("Area {0}. Could not recall callback !! Exception {1}", this, ex);
}
}
}
public override PlanSet CreatePlan(WorldState actual, Goal destiny)
{
int iter = 0;
PlanSet PlanSet = new PlanSet();
PriorityQueueB <pathrecnode> processing = new PriorityQueueB <pathrecnode>(new comparer());
List <WorldState> close = new List <WorldState>();
processing.Push(
new pathrecnode()
{
act = new List <Action>(),
WorldState = actual,
f = 0,
g = 0,
h = 0,
}
);
pathrecnode current = null;
while (processing.Count != 0)
{
current = processing.Pop();
if (destiny.Evaluate(current.WorldState) == true)
{
break;
}
if (close.Contains(current.WorldState))
{
continue;
}
else
{
close.Add(current.WorldState);
}
List <Action> acts = new List <Action>();
foreach (var item in Actions)
{
if (item.GetPreConditions(current.WorldState).isCompatibleSource(current.WorldState))
{
if (item.ProceduralPreConditions(current.WorldState))
{
acts.Add(item);
}
}
}
foreach (var item in acts)
{
WorldState ws = current.WorldState.Clone();
foreach (var item2 in item.GetEffects(current.WorldState).GetSymbols())
{
ws.SetSymbol(item2.Clone());
}
item.ApplyEffects(ws);
pathrecnode pathrec = new pathrecnode();
pathrec.WorldState = ws;
pathrec.act = new List <Action>(current.act.ToArray());
pathrec.act.Add(item);
pathrec.g += 1 + item.Cost;
pathrec.h = destiny.GetHeuristic(pathrec.WorldState);
//pathrec.WorldState.GetHeuristic(destiny.WorldState);
pathrec.f = pathrec.g + pathrec.h;
processing.Push(pathrec);
}
iter++;
if (iter > MaxIteration)
{
return(null);
}
Debug(processing, iter);
}
if (current != null)
{
foreach (var item in current.act)
{
PlanSet.Actions.Add(item);
System.Diagnostics.Debug.WriteLine(item.Name);
}
}
return(PlanSet);
}
public List <PathFinderNode> FindPath(Vector2i start, Vector2i end)
{
bool found = false;
bool stop = false;
int heuristicEstimate = 2;
int closeNodeCounter = 0;
int directionCount = _diagonals ? 8 : 4;
// Instead of clearing the grid each time, I change node state values and simply ignore the other values.
// It's faster than clearing the grid (not much, but it is).
_openNodeValue += 2;
_closeNodeValue += 2;
_open.Clear();
_close.Clear();
int location = EncodeLocation(start.X, start.Y);
int endLocation = EncodeLocation(end.X, end.Y);
ushort locationX = 0, locationY = 0;
_calcGrid[location].goneCost = 0;
_calcGrid[location].cost = 0 + heuristicEstimate;
_calcGrid[location].parentX = (ushort)start.X;
_calcGrid[location].parentY = (ushort)start.Y;
_calcGrid[location].state = _openNodeValue;
_open.Push(location);
while (_open.Count > 0 && !stop)
{
location = _open.Pop();
// Is it in closed list? means this node was already processed
if (_calcGrid[location].state == _closeNodeValue)
{
continue;
}
DecodeLocation(location, ref locationX, ref locationY);
if (location == endLocation)
{
_calcGrid[location].state = _closeNodeValue;
found = true;
break;
}
if (closeNodeCounter > _searchLimit)
{
// Evaluated nodes exceeded limit : path not found
Console.WriteLine("Pathfinder searchLimit exceed");
return(null);
}
// Let's calculate each successors
for (int i = 0; i < directionCount; ++i)
{
ushort newLocationX = (ushort)(locationX + _directions[i][0]);
ushort newLocationY = (ushort)(locationY + _directions[i][1]);
int newLocation = EncodeLocation(newLocationX, newLocationY);
int newGoneCost;
// Outside the grid?
if (newLocationX >= _gridSizeX || newLocationY >= _gridSizeY)
{
continue;
}
// Not crossable?
if (_grid[newLocation] == 0)
{
continue;
}
if (_avoidDiagonalCross)
{
//
// +----+----+----+
// | | | 3 |
// | | | |
// +----+----+----+
// | | 2 |XXXX| Diagonals are allowed,
// | | |XXXX| but going through 1, 2 then 3 should be avoided,
// +----+----+----+ because there are contiguous uncrossable cells.
// | | 1 |XXXX| (A square object cannot go from 2 to 3 for example, it will have to bypass the corner).
// | | |XXXX|
// +----+----+----+
//
if (i > 3)
{
if (_grid[EncodeLocation(locationX + _directions[i][0], locationY)] == 0 ||
_grid[EncodeLocation(locationX, locationY + _directions[i][1])] == 0)
{
continue;
}
}
}
if (_heavyDiagonals && i > 3)
{
newGoneCost = _calcGrid[location].goneCost + (int)(_grid[newLocation] * 2.41f);
}
else
{
newGoneCost = _calcGrid[location].goneCost + _grid[newLocation];
}
//Is it open or closed?
if (_calcGrid[newLocation].state == _openNodeValue || _calcGrid[newLocation].state == _closeNodeValue)
{
// The current node has less code than the previous? then skip this node
if (_calcGrid[newLocation].goneCost <= newGoneCost)
{
continue;
}
}
_calcGrid[newLocation].parentX = locationX;
_calcGrid[newLocation].parentY = locationY;
_calcGrid[newLocation].goneCost = newGoneCost;
// Heuristic : manhattan distance
int heuristic = heuristicEstimate * (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
_calcGrid[newLocation].cost = newGoneCost + heuristic;
//It is faster if we leave the open node in the priority queue
//When it is removed, it will be already closed, it will be ignored automatically
_open.Push(newLocation);
_calcGrid[newLocation].state = _openNodeValue;
}
closeNodeCounter++;
_calcGrid[location].state = _closeNodeValue;
}
if (found)
{
_close.Clear();
int posX = end.X;
int posY = end.Y;
PathFinderNodeFast tmpNode = _calcGrid[EncodeLocation(end.X, end.Y)];
PathFinderNode node = new PathFinderNode();
node.cost = tmpNode.cost;
node.goneCost = tmpNode.goneCost;
node.heuristic = 0;
node.parentX = tmpNode.parentX;
node.parentY = tmpNode.parentY;
node.x = end.X;
node.y = end.Y;
while (node.x != node.parentX || node.y != node.parentY)
{
_close.Add(node);
posX = node.parentX;
posY = node.parentY;
tmpNode = _calcGrid[EncodeLocation(posX, posY)];
node.cost = tmpNode.cost;
node.goneCost = tmpNode.goneCost;
node.heuristic = 0;
node.parentX = tmpNode.parentX;
node.parentY = tmpNode.parentY;
node.x = posX;
node.y = posY;
}
_close.Add(node);
// Path found
return(_close);
}
// Path not found
Console.WriteLine("Pathfinder path not found");
return(null);
}
public Path FindPath(short startCell, short endCell, bool diagonal, int movementPoints = -1)
{
bool flag = false;
PathNode[] matrix = new PathNode[561];
PriorityQueueB <short> priorityQueueB = new PriorityQueueB <short>((IComparer <short>) new Pathfinder.ComparePfNodeMatrix(matrix));
List <PathNode> list = new List <PathNode>();
MapPoint mapPoint1 = new MapPoint(startCell);
MapPoint pointB = new MapPoint(endCell);
short num1 = startCell;
int num2 = 0;
Path path;
if (movementPoints == 0)
{
path = Path.GetEmptyPath(this.CellsInformationProvider.Map, this.CellsInformationProvider.Map.Cells[(int)startCell]);
}
else
{
matrix[(int)num1].Cell = num1;
matrix[(int)num1].Parent = (short)-1;
matrix[(int)num1].G = 0.0;
matrix[(int)num1].F = (double)Pathfinder.EstimateHeuristic;
matrix[(int)num1].Status = NodeState.Open;
priorityQueueB.Push(num1);
while (priorityQueueB.Count > 0)
{
short cellId1 = priorityQueueB.Pop();
MapPoint mapPoint2 = new MapPoint(cellId1);
if (matrix[(int)cellId1].Status != NodeState.Closed)
{
if ((int)cellId1 != (int)endCell)
{
if (num2 <= Pathfinder.SearchLimit)
{
for (int index = 0; index < (!diagonal ? 4 : 8); ++index)
{
MapPoint nearestCellInDirection = mapPoint2.GetNearestCellInDirection(Pathfinder.Directions[index]);
if (nearestCellInDirection != null)
{
short cellId2 = nearestCellInDirection.CellId;
if (((int)cellId2 < 0 ? 0 : ((long)cellId2 < 560L ? 1 : 0)) != 0 && MapPoint.IsInMap(nearestCellInDirection.X, nearestCellInDirection.Y) && this.CellsInformationProvider.IsCellWalkable(cellId2))
{
double num3 = matrix[(int)cellId1].G + 1.0;
if ((matrix[(int)cellId2].Status == NodeState.Open || matrix[(int)cellId2].Status == NodeState.Closed ? (matrix[(int)cellId2].G > num3 ? 1 : 0) : 1) != 0)
{
matrix[(int)cellId2].Cell = cellId2;
matrix[(int)cellId2].Parent = cellId1;
matrix[(int)cellId2].G = num3;
matrix[(int)cellId2].H = Pathfinder.GetHeuristic(nearestCellInDirection, pointB);
matrix[(int)cellId2].F = num3 + matrix[(int)cellId2].H;
priorityQueueB.Push(cellId2);
matrix[(int)cellId2].Status = NodeState.Open;
}
}
}
}
++num2;
matrix[(int)cellId1].Status = NodeState.Closed;
}
else
{
path = Path.GetEmptyPath(this.CellsInformationProvider.Map, this.CellsInformationProvider.Map.Cells[(int)startCell]);
goto label_23;
}
}
else
{
matrix[(int)cellId1].Status = NodeState.Closed;
flag = true;
break;
}
}
}
if (flag)
{
PathNode pathNode;
for (pathNode = matrix[(int)endCell]; (int)pathNode.Parent != -1; pathNode = matrix[(int)pathNode.Parent])
{
list.Add(pathNode);
}
list.Add(pathNode);
}
list.Reverse();
path = (movementPoints <= 0 ? 1 : (list.Count + 1 <= movementPoints ? 1 : 0)) != 0 ? new Path(this.CellsInformationProvider.Map, Enumerable.Select <PathNode, Cell>((IEnumerable <PathNode>)list, (Func <PathNode, Cell>)(entry => this.CellsInformationProvider.Map.Cells[(int)entry.Cell]))) : new Path(this.CellsInformationProvider.Map, Enumerable.Select <PathNode, Cell>(Enumerable.Take <PathNode>((IEnumerable <PathNode>)list, movementPoints + 1), (Func <PathNode, Cell>)(entry => this.CellsInformationProvider.Map.Cells[(int)entry.Cell])));
}
label_23:
return(path);
}