public AlgorithmState(ObservableCollection <PFNode> squares, List <PFNode> available, PFNode currentlyChecking, AlgorithmState last)
{
Squares = squares;
CurrentlyChecking = currentlyChecking;
Available = available;
Last = last;
}
// This is heftily f****d up. Seriously needs fixing.
public PFNode getNodeNearestCover()
{
//float nearestDistance = float.MaxValue;
//PFNode nearestNode;
if (currentNode.type == PFNodeType.Crouch || currentNode.type == PFNodeType.Stand)
{
return(currentNode);
}
foreach (PFNodeEntry node in currentNode.Nodes)
{
if (node.node.type == PFNodeType.Stand || node.node.type == PFNodeType.Crouch)
{
// If node is cover
Vector3 startPos = transform.position;
Vector3.MoveTowards(startPos, node.node.transform.position, 1);
if (!Physics.Linecast(startPos, node.node.transform.position))
{
choice = node.node;
return(node.node);
}
}
}
return(currentNode); // Untill we can have recursive search systems.
}
private int GetManhattenDistance(PFNode nodeA, PFNode nodeB)
{
int ix = Mathf.Abs(nodeA.GridX - nodeB.GridX);
int iy = Mathf.Abs(nodeA.GridY - nodeB.GridY);
return(ix + iy);
}
public void SetupGroup()
{
PFNode[] PFNodes = GetComponentsInChildren <PFNode>();
//print (PFNodes.Length);
for (int i = 0; i < PFNodes.Length; i++)
{
PFNodes[i].Nodes = new PFNodeEntry[PFNodes.Length - 1];
for (int o = 0; o < PFNodes[i].Nodes.Length; o++)
{
PFNodes[i].Nodes[o] = new PFNodeEntry();
}
//print("Working at "+PFNodes[i].name+".");
for (int o = 0; o < PFNodes.Length; o++)
{
//print("Attempting to connect "+i+" and "+o+".");
PFNode n0 = PFNodes[o];
if (o > i)
{
PFNodes[i].Nodes[o - 1].node = PFNodes[o];
PFNodes[i].Nodes[o - 1].distance = Vector3.Distance(PFNodes[i].transform.position, PFNodes[o].transform.position);
}
else if (o < i)
{
PFNodes[i].Nodes[o].node = PFNodes[o];
PFNodes[i].Nodes[o].distance = Vector3.Distance(PFNodes[i].transform.position, PFNodes[o].transform.position);
}
}
}
}
public PFNode UpdateStatus(int newStatus) //Since List Returns Copy of Node
//We use this to replace existing node.
{
PFNode newNode = this;
newNode.Status = newStatus;
return(newNode);
}
public float riskAssessment(GameObject[] enemies, PFNode otherNode)
{
foreach (GameObject enemy in enemies) {
riskFactor += Vector3.Distance(enemy.transform.position, node.transform.position);
riskFactor += Vector3.Distance(enemy.transform.position, otherNode.transform.position);
}
riskFactor += Vector3.Distance(node.transform.position, otherNode.transform.position);
return riskFactor;
}
private void FindPath(Vector3 startPosition, Vector3 targetPosition)
{
PFNode startNode = _grid.GetNodeFromWorldPosition(startPosition);
PFNode targetNode = _grid.GetNodeFromWorldPosition(targetPosition);
// The list for all the checked nodes, and get deleted each time
List <PFNode> openList = new List <PFNode>();
// The hashset of the relevant nodes for the path
HashSet <PFNode> closedList = new HashSet <PFNode>();
openList.Add(startNode);
while (openList.Count > 0)
{
PFNode currentNode = openList[0];
for (int i = 1; i < openList.Count; i++)
{
if (openList[i].FCost < currentNode.FCost || openList[i].FCost == currentNode.FCost &&
openList[i].hCost < currentNode.hCost)
{
currentNode = openList[i];
}
}
openList.Remove(currentNode);
closedList.Add(currentNode);
if (currentNode == targetNode)
{
GetFinalPath(startNode, targetNode);
}
foreach (PFNode neighborNode in _grid.GetNeighboringNodes(currentNode))
{
if (neighborNode.IsObstacle || closedList.Contains(neighborNode))
{
continue;
}
int moveCost = currentNode.gCost + GetManhattenDistance(currentNode, neighborNode);
if (moveCost < neighborNode.gCost || !openList.Contains(neighborNode))
{
neighborNode.gCost = moveCost;
neighborNode.hCost = GetManhattenDistance(neighborNode, targetNode);
neighborNode.Parent = currentNode;
if (!openList.Contains(neighborNode))
{
openList.Add(neighborNode);
}
}
}
}
}
public float riskAssessment(GameObject[] enemies, PFNode otherNode)
{
foreach (GameObject enemy in enemies)
{
riskFactor += Vector3.Distance(enemy.transform.position, node.transform.position);
riskFactor += Vector3.Distance(enemy.transform.position, otherNode.transform.position);
}
riskFactor += Vector3.Distance(node.transform.position, otherNode.transform.position);
return(riskFactor);
}
/// <summary>
/// Gets the node closest from any enemy combatants, inclusive of current nodea.
///
/// FUTURE: Add calculations for safety based on effective range of the weapon they are holding.
/// e.g., One is much safer ten meters away from a sniper than one half-kilometer away.
/// </summary>
/// <returns>
/// The most dangerous node.
/// </returns>
public PFNode getNodeClosestToEnemies(GameObject[] enemies, Faction allegiance = Faction.Evil)
{
float leastDangerous = 0f;
int index = -1;
int i = 0;
foreach (GameObject e in enemies)
{
// Change the != to whatever the faction relationship system is.
if (e.GetComponent <Enemy>().faction != allegiance)
{
leastDangerous +=
(currentNode.transform.position - e.transform.position).sqrMagnitude;
}
}
if (debugMode)
{
print("Risk for " + currentNode.name + " is " + leastDangerous);
}
foreach (PFNodeEntry node in currentNode.Nodes)
{
float riskFactor = 0;
if (debugMode)
{
foreach (GameObject g in enemies)
{
print(g.name);
}
}
foreach (GameObject e in enemies)
{
if (e.GetComponent <Enemy>().faction != allegiance)
{
riskFactor +=
(node.node.transform.position - e.transform.position).sqrMagnitude;
}
//if (debugMode) print ("Calculated for " + e.name + " near " + node.node.gameObject.name);
}
if (debugMode)
{
print("Risk for " + node.node.name + " is " + riskFactor);
}
if (riskFactor < leastDangerous)
{
index = i;
leastDangerous = riskFactor;
}
i++;
}
choice = (index == -1) ? currentNode : currentNode.Nodes[index].node;
return((index == -1) ? currentNode : currentNode.Nodes[index].node);
}
public PFNode Dequeue()
{
if (List.Count == 0)
{
return(null);
}
PFNode top = List[0];
List.RemoveAt(0);
return(top);
}
public PFNode Take(Vector2Int pos)
{
for (int i = 0; i < List.Count; i++)
{
if (List[i].pos == pos)
{
PFNode ret = List[i];
List.RemoveAt(i);
return(ret);
}
}
return(null);
}
private void GetFinalPath(PFNode startNode, PFNode endNode)
{
List <PFNode> finalPath = new List <PFNode>();
PFNode currentNode = endNode;
while (currentNode != startNode)
{
finalPath.Add(currentNode);
currentNode = currentNode.Parent;
}
Debug.Log("found path!");
finalPath.Reverse();
_grid.FinalPath = finalPath;
}
public void OnSceneGUI()
{
PFNode[] PFNs = ((PFNodeAmorphousGroup)target).transform.gameObject.GetComponentsInChildren <PFNode>();
for (int i = 0; i < PFNs.Length; i++)
{
PFNode p = PFNs[i];
p.transform.position = Handles.PositionHandle(p.transform.position, p.transform.rotation);
//foreach (PFNodeEntry e in p.Nodes) {
// Handles.DrawLine(e.node.transform.position, p.transform.position);
//}
Handles.Label(p.transform.position, "Node " + i.ToString());
}
if (GUI.changed)
{
EditorUtility.SetDirty(target);
}
}
/// <summary>
/// Gets the nearest node to the current node.
/// </summary>
/// <returns>
/// The node nearest the current node.
/// </returns>
public PFNode getNodeNearest()
{
float distance = float.MaxValue; // Should be safe.
int index = -1;
int i = 0;
foreach (PFNodeEntry E in currentNode.Nodes)
{
if (E.distance < distance)
{
distance = E.distance;
index = i;
}
i++;
}
choice = (index == -1) ? currentNode : currentNode.Nodes[index].node;
return((index == -1) ? currentNode : currentNode.Nodes[index].node);
}
private int InsertSearch(int start, int end, int fcost)
{
if (start == end)
{
return(start);
}
int mid = (start + end) / 2;
int comp = PFNode.Compare(fcost, List[mid]);
if (comp <= 0)
{
return(InsertSearch(start, mid, fcost));
}
else
{
return(InsertSearch(mid + 1, end, fcost));
}
}
/// <summary>
/// Gets the node where one is most likely to be able to blow someone's brains out.
/// Basically, the node with the most teammates, and the most enemeies nearby.
/// </summary>
/// <returns>
/// The most dangerous node.
/// </returns>
public PFNode getNodeMostDangerous(GameObject[] enemies, Faction allegiance = Faction.Evil)
{
float leastDangerous = 0f;
int index = -1;
int i = 0;
foreach (GameObject e in enemies) {
// Change the != to whatever the faction relationship system is.
if (e.GetComponent<Enemy>().faction != allegiance) leastDangerous +=
(currentNode.transform.position - e.transform.position).sqrMagnitude;
}
if (debugMode) print ("Risk for " + currentNode.name + " is "+leastDangerous);
foreach (PFNodeEntry node in currentNode.Nodes) {
float riskFactor = 0;
if (debugMode) foreach (GameObject g in enemies)print (g.name);
foreach (GameObject e in enemies) {
// This is the fancy bit where you calculate where to run and hide.
if (e.GetComponent<Enemy>() is ShootingEnemy) {
//float thisCombatantsRisk;
if (e != gameObject.GetComponent<Enemy>()) {
if (e.GetComponent<Enemy>().faction != allegiance) riskFactor +=
(node.node.transform.position-e.transform.position).sqrMagnitude;
}
} else if (e.GetComponent<Enemy>() is PlayerCombatant) {
if (e.GetComponent<Enemy>().faction != allegiance) riskFactor +=
(node.node.transform.position- e.transform.position).sqrMagnitude;
} else {
if (e.GetComponent<Enemy>().faction != allegiance) riskFactor +=
(node.node.transform.position- e.transform.position).sqrMagnitude;
}
//if (debugMode) print ("Calculated for " + e.name + " near " + node.node.gameObject.name);
}
if (debugMode) print ("Risk for " + node.node.name + " is "+riskFactor);
if (riskFactor < leastDangerous) {
index = i;
leastDangerous = riskFactor;
}
i++;
}
choice=(index == -1) ? currentNode : currentNode.Nodes[index].node;
return (index == -1) ? currentNode : currentNode.Nodes[index].node;
}
/// <summary>
/// Gets the node closest from any enemy combatants, inclusive of current nodea.
///
/// FUTURE: Add calculations for safety based on effective range of the weapon they are holding.
/// e.g., One is much safer ten meters away from a sniper than one half-kilometer away.
/// </summary>
/// <returns>
/// The most dangerous node.
/// </returns>
public PFNode getNodeClosestToEnemies(GameObject[] enemies, Faction allegiance = Faction.Evil)
{
float leastDangerous = 0f;
int index = -1;
int i = 0;
foreach (GameObject e in enemies) {
// Change the != to whatever the faction relationship system is.
if (e.GetComponent<Enemy>().faction != allegiance) leastDangerous +=
(currentNode.transform.position- e.transform.position).sqrMagnitude;
}
if (debugMode) print ("Risk for " + currentNode.name + " is "+leastDangerous);
foreach (PFNodeEntry node in currentNode.Nodes) {
float riskFactor = 0;
if (debugMode) foreach (GameObject g in enemies)print (g.name);
foreach (GameObject e in enemies) {
if (e.GetComponent<Enemy>().faction != allegiance) riskFactor +=
(node.node.transform.position - e.transform.position).sqrMagnitude;
//if (debugMode) print ("Calculated for " + e.name + " near " + node.node.gameObject.name);
}
if (debugMode) print ("Risk for " + node.node.name + " is "+riskFactor);
if (riskFactor < leastDangerous) {
index = i;
leastDangerous = riskFactor;
}
i++;
}
choice=(index == -1) ? currentNode : currentNode.Nodes[index].node;
return (index == -1) ? currentNode : currentNode.Nodes[index].node;
}
// This is heftily f****d up. Seriously needs fixing.
public PFNode getNodeNearestCover()
{
//float nearestDistance = float.MaxValue;
//PFNode nearestNode;
if (currentNode.type == PFNodeType.Crouch || currentNode.type == PFNodeType.Stand) return currentNode;
foreach (PFNodeEntry node in currentNode.Nodes) {
if (node.node.type == PFNodeType.Stand || node.node.type == PFNodeType.Crouch) {
// If node is cover
Vector3 startPos = transform.position;
Vector3.MoveTowards(startPos, node.node.transform.position, 1);
if (!Physics.Linecast(startPos, node.node.transform.position)) {
choice=node.node;
return node.node;
}
}
}
return currentNode; // Untill we can have recursive search systems.
}
/// <summary>
/// Gets the nearest node to the current node.
/// </summary>
/// <returns>
/// The node nearest the current node.
/// </returns>
public PFNode getNodeNearest()
{
float distance = float.MaxValue; // Should be safe.
int index = -1;
int i = 0;
foreach (PFNodeEntry E in currentNode.Nodes) {
if (E.distance < distance) {
distance = E.distance;
index = i;
}
i++;
}
choice=(index == -1) ? currentNode : currentNode.Nodes[index].node;
return (index == -1) ? currentNode : currentNode.Nodes[index].node;
}
/// <summary>
/// Gets the node where one is most likely to be able to blow someone's brains out.
/// Basically, the node with the most teammates, and the most enemeies nearby.
/// </summary>
/// <returns>
/// The most dangerous node.
/// </returns>
public PFNode getNodeMostDangerous(GameObject[] enemies, Faction allegiance = Faction.Evil)
{
float leastDangerous = 0f;
int index = -1;
int i = 0;
foreach (GameObject e in enemies)
{
// Change the != to whatever the faction relationship system is.
if (e.GetComponent <Enemy>().faction != allegiance)
{
leastDangerous +=
(currentNode.transform.position - e.transform.position).sqrMagnitude;
}
}
if (debugMode)
{
print("Risk for " + currentNode.name + " is " + leastDangerous);
}
foreach (PFNodeEntry node in currentNode.Nodes)
{
float riskFactor = 0;
if (debugMode)
{
foreach (GameObject g in enemies)
{
print(g.name);
}
}
foreach (GameObject e in enemies)
{
// This is the fancy bit where you calculate where to run and hide.
if (e.GetComponent <Enemy>() is ShootingEnemy)
{
//float thisCombatantsRisk;
if (e != gameObject.GetComponent <Enemy>())
{
if (e.GetComponent <Enemy>().faction != allegiance)
{
riskFactor +=
(node.node.transform.position - e.transform.position).sqrMagnitude;
}
}
}
else if (e.GetComponent <Enemy>() is PlayerCombatant)
{
if (e.GetComponent <Enemy>().faction != allegiance)
{
riskFactor +=
(node.node.transform.position - e.transform.position).sqrMagnitude;
}
}
else
{
if (e.GetComponent <Enemy>().faction != allegiance)
{
riskFactor +=
(node.node.transform.position - e.transform.position).sqrMagnitude;
}
}
//if (debugMode) print ("Calculated for " + e.name + " near " + node.node.gameObject.name);
}
if (debugMode)
{
print("Risk for " + node.node.name + " is " + riskFactor);
}
if (riskFactor < leastDangerous)
{
index = i;
leastDangerous = riskFactor;
}
i++;
}
choice = (index == -1) ? currentNode : currentNode.Nodes[index].node;
return((index == -1) ? currentNode : currentNode.Nodes[index].node);
}
public bool accessibilityAssessment(PFNode otherNode)
{
return accessible = !Physics.Linecast(node.transform.position, otherNode.transform.position);
}
// TODO: update UI components affected (put it in Calculate function for each of the squares)
private async Task <List <PFNode> > CalculateSquareSorroundings(PFNode square, bool calcCosts = true)
{
var x = square.X;
var y = square.Y;
List <Task> tasks = new List <Task>();
List <PFNode> toR = new List <PFNode>();
// this list is any square sorrounding the square that we need to check
List <(int, int)> toCheck = new List <(int, int)>()
{
//(x + 1, y + 1),
(x, y + 1),
//(x - 1, y + 1),
(x + 1, y),
(x, y),
(x - 1, y),
//(x + 1, y - 1),
(x, y - 1),
//(x - 1, y - 1),
};
// because we are removing elements in toCheck I'd like to avoid using the same list
foreach (var t in toCheck.ToList())
{
if (t.Item1 < 0 || t.Item1 > 52 || t.Item2 < 0 || t.Item2 > 52)
{
toCheck.Remove(t);
}
else if (!SquareIsWalkable(SquaresList[t.Item1 * 53 + t.Item2]))
{
toCheck.Remove(t);
}
}
if (calcCosts)
{
if (SelectedAlgorithmType == AlgorithmType.AStar)
{
foreach (var t in toCheck)
{
PFNode curr = SquaresList[t.Item1 * 53 + t.Item2];
tasks.Add(Task.Run(curr.AStarCalculateCosts));
toR.Add(curr);
}
}
else if (SelectedAlgorithmType == AlgorithmType.Djikstras)
{
foreach (var t in toCheck)
{
PFNode curr = SquaresList[t.Item1 * 53 + t.Item2];
tasks.Add(Task.Run(curr.DjikstrasCalculateCosts));
toR.Add(curr);
}
}
else
{
// THIS IS TEMPORARY
foreach (var t in toCheck)
{
PFNode curr = SquaresList[t.Item1 * 53 + t.Item2];
tasks.Add(Task.Run(curr.AStarCalculateCosts));
toR.Add(curr);
}
}
}
else
{
foreach (var k in toCheck)
{
toR.Add(SquaresList[k.Item1 * 53 + k.Item2]);
}
return(toR);
}
await Task.WhenAll(tasks);
return(toR);
}
public PFNode(INavable cube, PFNode prevNode, int cost)
{
this.cube = cube;
this.prevNode = prevNode;
this.cost = cost;
}
/// <summary>
/// maxDistance로 갈수 있는 큐브들을 BFS로 찾아 OnServe콜백함수의 PFPath인자로 돌려줍니다.
/// </summary>
/// <param name="maxDistance">BFS로 찾을 최대 거리</param>
/// <param name="OnServe">함수가 끝나면 호출할 함수를 전달하세요. 함수의 인자로 Path가 전달됩니다.</param>
/// <param name="cubeIgnore">Path에 포함시키지 않을 Predicate</param>
/// <returns></returns>
private IEnumerator BFSPathfinding(
INavable start, List <INavable> navables, int maxDistance,
Action <List <PFPath> > OnServe, Func <INavable, bool> cubeIgnore)
{
OnSearchBegin();
// 나중에 cost가 정해진 노드들만 path로 만들기 위함
List <PFNode> table = new List <PFNode>();
// BFS: Initialization
Queue <PFNode> queue = new Queue <PFNode>();
PFNode startNode = new PFNode(start, null, 0);
queue.Enqueue(startNode);
table.Add(startNode);
// BFS: Traversal
int maxLoop = 40;
int currLoop = 0;
while (queue.Count > 0)
{
PFNode currNode = queue.Dequeue();
if (currNode.cost >= maxDistance)
{
continue;
}
List <INavable> neighborCubes = currNode.cube.Neighbors;
foreach (var neighborCube in neighborCubes)
{
if (cubeIgnore(neighborCube))
{
continue;
}
if (table.Any(node => node.cube == neighborCube))
{
continue; // 이미 다른 Path에 있음
}
PFNode newNode = new PFNode(neighborCube, currNode, currNode.cost + 1);
queue.Enqueue(newNode);
table.Add(newNode);
}
currLoop++;
if (currLoop >= maxLoop)
{
currLoop = 0;
yield return(null);
}
}
// Path Construction
List <PFPath> paths = new List <PFPath>();
currLoop = 0;
foreach (var destination in table)
{
PFPath path = new PFPath(start, destination);
path.Add(destination);
PFNode currNode = destination;
while (currNode.prevNode != null)
{
path.Add(currNode.prevNode);
currNode = currNode.prevNode;
}
path.Reverse();
paths.Add(path);
currLoop++;
if (currLoop >= maxLoop)
{
currLoop = 0;
yield return(null);
}
}
// return by Calling Callback Function
OnServe(paths);
OnSearchEnd();
}
public static int Compare(int a, PFNode b)
{
return(a - b.GetFcost());
}
public static int Compare(PFNode a, PFNode b)
{
return(a.GetFcost() - b.GetFcost());
}
public List <Vector2> FindPath(Vector2 start, Vector2 end)
{
#region Setup
//Stop if end goal is impossible
if (GridManager.Instance.ContainsGridObject(true, (int)end.x, (int)end.y))
{
Debug.LogWarning("AStar Failed. End Location is blocked");
return(null);
}
//Clearing Before Start
while (traversedCoordinates.Count > 0)
{
nodes[traversedCoordinates.Pop()].Clear();
}
Debug.Log("FindPath Called. Start: (" + start.x + "," + start.y + ") & " + "End: (" + end.x + "," + end.y + ")");
//Reset Values
nodeOpenValue += 2; //This allows for nodes to be reset without looping through grid
nodeCloseValue += 2;
openLocations.Clear();
//If Created Before GridManager is loaded
if (nodes.Length == 0)
{
SetUpGridAndQueue();
}
//Convert From Vector to Location
Location myLocation = new Location((int)start.y * GridManager.Instance.GridWidth + (int)start.x, 0);
Location endLocation = new Location((int)end.y * GridManager.Instance.GridWidth + (int)end.x, 0);
//First Node To Branch From
PFNode firstNode = new PFNode();
firstNode.G = 0;
firstNode.F = 1;
firstNode.PX = (ushort)start.x;
firstNode.PY = (ushort)start.y;
firstNode.PZ = 0;
firstNode.Status = nodeOpenValue;
//Setting Jump Length
if (GridManager.Instance.ContainsGridObject(true, (int)start.x, (int)start.y - 1))
{
firstNode.JumpLength = 0;
}
else
{
firstNode.JumpLength = (short)(characterJumpHeight * 2);
}
Debug.Log("GridWidth = " + GridManager.Instance.GridWidth + ". mylocation.xy = " + myLocation.xy
+ ". Nodes Array Length = " + nodes.Length);
//Adding Start Location to Stack
nodes[myLocation.xy].Add(firstNode);
traversedCoordinates.Push(myLocation.xy);
openLocations.Push(myLocation);
#endregion
bool found = false;
long iterationCount = 0;
int[,] direction = new int[8, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
};
//Loop Through Priority Queue
while (openLocations.Count > 0) //Add Other Stop Condition Maybe?
{
Location current = openLocations.Pop();
if (nodes[current.xy][current.z].Status == nodeCloseValue) //Ignore Visited
{
continue;
}
int currentX = current.xy % GridManager.Instance.GridWidth;
int currentY = current.xy / GridManager.Instance.GridWidth; //Int division truncates off x portion
//Found Target Path!
if (current.xy == endLocation.xy)
{
nodes[current.xy][current.z] = nodes[current.xy][current.z].UpdateStatus(nodeCloseValue);
found = true;
break;
}
//Search Limit
if (iterationCount > iterationSearchLimit)
{
Debug.LogWarning("AStar Pathfinding Failed Due to Search Limit");
return(null);
}
//Find Successors
for (int i = 0; i < 8; ++i)
{
int successorX = (ushort)(currentX + direction[i, 0]);
int successorY = (ushort)(currentY + direction[i, 1]);
int successorXY = successorY * GridManager.Instance.GridWidth + successorX;
//Ignore non-navigable block
if (HasBlock(successorX, successorY))
{
continue;
}
//Ignore moving diagonal edge case- blocks perpendicular sides
if (direction[i, 0] != 0 && direction[i, 1] != 0)
{
if (HasBlock(currentX + direction[i, 0], currentY) &&
HasBlock(currentX, currentY + direction[i, 1]))
{
continue;
}
}
bool onGround = HasBlock(successorX, successorY - 1);
bool atCeiling = HasBlock(successorX, successorY + 1);
bool currentOnGround = HasBlock(currentX, currentY - 1);
int jumpLength = nodes[current.xy][current.z].JumpLength; //Grabs Old
int newJumpLength = -1;
//JumpLength is how long in the air, at max jump height, JumpLength is maxJumpHeight * 2
//This gives granularity to the stage of jump length
//Even JumpLength values means the character could go Up, Down, Right, Left
//Odd JumpLength values means the character could go Up, Down
//////////////////////////////////////////
//--Find New Jump Length for Successor--//
//////////////////////////////////////////
//Reset to Zero
if (onGround)
{
newJumpLength = 0;
}
//Ceiling
else if (atCeiling)
{
if (successorX == currentX) //Fall Down
{
newJumpLength = (short)Mathf.Max(characterJumpHeight * 2, jumpLength + 2);
}
else //Slide Horizontal
{
newJumpLength = (short)Mathf.Max(characterJumpHeight * 2 + 1, jumpLength + 1);
}
}
//Going Up
else if (successorY > currentY)
{
if (jumpLength < 2) //Boost! Guarantees next move will go --- not -- Up
{
newJumpLength = 2;
}
else
{
newJumpLength = NextEvenNumber(jumpLength);
}
}
//Going Down
else if (successorY < currentY)
{
newJumpLength = (short)Mathf.Max(characterJumpHeight * 2, NextEvenNumber(jumpLength));
}
//In-Air Side to Side
else if (successorX != currentX)
{
newJumpLength = jumpLength + 1;
}
//////////////////////////////////////////////////
//--Ignore Poor Successors Based On JumpLength--//
//////////////////////////////////////////////////
//If Odd, Ignore Right and Left Successors
if (jumpLength % 2 != 0 && successorX != currentX)
{
continue;
}
//If Falling, Make Sure Not Going Up
if (jumpLength >= characterJumpHeight * 2 && successorY > currentY)
{
continue;
}
//If Falling fast, Make Sure Not Going Sideways
if (newJumpLength >= characterJumpHeight * 2 + blocksFallenUntilCancelSideways &&
successorX != currentX)
{
continue;
}
if (onGround && !currentOnGround && successorX != currentX)
{
continue;
}
//If revisiting, only continue if it can add something new to the table
/*Debug.Log("SuccessorXY is " + successorXY + ". X = " + successorX + ". Y = " + successorY
+ ". CurrentX = " + currentX + ". CurrentY = " + currentY
+ ". Direction is "+direction[i,0]+","+direction[i,1]
+ ". NewJumpLength is"+newJumpLength); */
if (nodes[successorXY].Count > 0)
{
int lowestJump = short.MaxValue;
bool visitedCouldMoveSideways = false;
for (int j = 0; j < nodes[successorXY].Count; ++j)
{
if (nodes[successorXY][j].JumpLength < lowestJump)
{
lowestJump = nodes[successorXY][j].JumpLength;
}
if (nodes[successorXY][j].JumpLength % 2 == 0 &&
nodes[successorXY][j].JumpLength < characterJumpHeight * 2 + blocksFallenUntilCancelSideways)
{
visitedCouldMoveSideways = true;
}
}
//Ignore if already visited node has shorter jump length and provides more insight
if (lowestJump <= newJumpLength &&
(newJumpLength % 2 != 0 ||
newJumpLength >= characterJumpHeight * 2 + blocksFallenUntilCancelSideways ||
visitedCouldMoveSideways))
{
continue;
}
}
////////////////////////////////////
//--Create and Add Node To Queue--//
////////////////////////////////////
//Calculate costs
int successorCost = nodes[current.xy][current.z].G + (int)(newJumpLength * jumpDeterrentMultiplier);
int distToGoal = (int)(Math.Sqrt(Math.Pow((successorX - end.x), 2) + Math.Pow((successorY - end.y), 2)));
//Create Node
PFNode newNode = new PFNode();
newNode.JumpLength = newJumpLength;
newNode.PX = currentX;
newNode.PY = currentY;
newNode.PZ = current.z;
newNode.G = successorCost;
newNode.F = successorCost + distToGoal;
newNode.Status = nodeOpenValue;
if (nodes[successorXY].Count == 0)
{
traversedCoordinates.Push(successorXY);
}
nodes[successorXY].Add(newNode);
openLocations.Push(new Location(successorXY, nodes[successorXY].Count - 1));
}
//After adding all possible successors, mark current node as closed
nodes[current.xy][current.z] = nodes[current.xy][current.z].UpdateStatus(nodeCloseValue);
iterationCount++;
}
if (found)
{
List <Vector2> path = new List <Vector2>();
int posX = (int)end.x;
int posY = (int)end.y;
PFNode fPrevNodeTmp = new PFNode();
PFNode fNodeTmp = nodes[endLocation.xy][0];
Vector2 fNode = end;
Vector2 fPrevNode = end;
int parentXY = fNodeTmp.PY * GridManager.Instance.GridWidth + fNodeTmp.PX;
//Recursively Build Path
while (fNode.x != fNodeTmp.PX || fNode.y != fNodeTmp.PY)
{
PFNode fNextNodeTmp = nodes[parentXY][fNodeTmp.PZ];
//Filters out redundant nodes
if ((path.Count == 0) ||
(fNodeTmp.JumpLength == 3) ||
(fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0) || //mark jumps starts
(fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0) || //mark landings
(fNode.y > path[path.Count - 1].y && fNode.y > fNodeTmp.PY) ||
(fNode.y < path[path.Count - 1].y && fNode.y < fNodeTmp.PY) ||
((HasBlock(fNode.x - 1, fNode.y) || HasBlock(fNode.x + 1, fNode.y)) &&
fNode.y != path[path.Count - 1].y && fNode.x != path[path.Count - 1].x))
{
path.Add(fNode);
}
fPrevNode = fNode;
posX = fNodeTmp.PX;
posY = fNodeTmp.PY;
fPrevNodeTmp = fNodeTmp;
fNodeTmp = fNextNodeTmp;
parentXY = fNodeTmp.PY * GridManager.Instance.GridWidth + fNodeTmp.PX;
fNode = new Vector2(posX, posY);
}
path.Add(fNode);
return(path);
}
Debug.LogWarning("AStar Pathfinding failed. Could not find path to goal. (" + end.x + "," + end.y + ")");
return(new List <Vector2>());
}
public bool accessibilityAssessment(PFNode otherNode)
{
return(accessible = !Physics.Linecast(node.transform.position, otherNode.transform.position));
}
public async Task AStarAlgorithmSteppedAsync(AlgorithmState last = null)
{
// first time check
if (last is null)
{
last = new AlgorithmState(new ObservableCollection <PFNode>(SquaresList), new List <PFNode>()
{
PFNode.StartPoint
}, PFNode.StartPoint, null);
Debug.WriteLine($"Last:{Environment.NewLine}{last}");
PFNode.StartPoint.Visited = true;
PFNode.StartPoint.PreviousNode = null;
States.Add(CurrentState);
}
else
{
last = CurrentState;
}
// end condition
if (last.CurrentlyChecking == PFNode.EndPoint)
{
Debug.WriteLine($"Solution found");
solved = true;
var curr = PFNode.EndPoint;
for (; ;)
{
if (curr.PreviousNode is null)
{
break;
}
if (curr != PFNode.StartPoint && curr != PFNode.EndPoint)
{
curr.VisualType = VisualSquareType.FinishPath;
}
curr = curr.PreviousNode;
}
return;
}
Debug.WriteLine($"Last checked square is not the endpoint, continue");
// calculate current square
PFNode currentlyChecking = last.Available[0];
Debug.WriteLine($"Currently checking the square {currentlyChecking}");
currentlyChecking.Visited = true;
// get valid sorroundings
List <PFNode> sorroundings = await CalculateSquareSorroundings(currentlyChecking);
Debug.WriteLine($"Got sorroundings of {currentlyChecking}:");
sorroundings = RemoveVisited(sorroundings);
sorroundings.ForEach(x =>
{
if (x != PFNode.EndPoint && x != PFNode.StartPoint)
{
x.VisualType = VisualSquareType.Sorrounding;
}
});
sorroundings.ForEach(x =>
{
if (x.PreviousNode is null)
{
x.PreviousNode = currentlyChecking;
}
});
Debug.WriteLine($"Removed visited squares");
last.Available.Remove(currentlyChecking);
last.Available = last.Available.Distinct(new AStarSquareComparer()).ToList();
var newAvailable = new List <PFNode>(last.Available);
newAvailable.AddRange(sorroundings);
RemoveVisited(newAvailable);
newAvailable.Sort(new AStarSquareComparer());
Debug.WriteLine($"Added last's sorroundings and removed all visited.");
CurrentState = new AlgorithmState(SquaresList, newAvailable, currentlyChecking, last);
States.Add(CurrentState);
if (last.CurrentlyChecking != PFNode.StartPoint && last.CurrentlyChecking != PFNode.EndPoint)
{
last.CurrentlyChecking.VisualType = VisualSquareType.Visited;
}
if (currentlyChecking != PFNode.StartPoint && currentlyChecking != PFNode.EndPoint)
{
currentlyChecking.VisualType = VisualSquareType.Visited;
}
Debug.WriteLine("");
Debug.WriteLine($"CurrentAmountOfSquares = {newAvailable.Count}");
Step++;
MaxStep++;
//if (currentlyChecking != PFNode.StartPoint)
//{
// currentlyChecking.PreviousNode = last.CurrentlyChecking;
//}
//else
//{
// currentlyChecking.PreviousNode = null;
//}
}
private bool SquareIsWalkable(PFNode s) => s.Type != SquareType.Bomb && s.Type != SquareType.Wall;
public static int Compare(PFNode a, int b)
{
return(a.GetFcost() - b);
}
public List <Vector2> FindPath(Vector2Int from, Vector2Int to)
{
Debug.Log(string.Format("Finding path from ({0},{1}) to ({2},{3})", from.x, from.y, to.x, to.y));
PFNode start = CreateInstance <PFNode>();
start.pos = from;
start.gcost = 0;
start.hcost = Dist(start.pos, to);
start.backpath = null;
Queue.Enqueue(start);
int loops = 0;
while (true)
{
if (loops >= 500)
{
Debug.LogError("TOO MANY LOOPS! BREAKING.");
return(null);
}
PFNode cur = Queue.Dequeue();
if (cur == null)
{
Debug.LogError("CAN'T FIND PATH");
return(null);
}
Closed.Add(cur);
if (cur.pos == to)
{
Debug.Log("FOUND DESTINATION");
break;
}
int newGcost = cur.gcost + 1;
for (int i = 0; i < 4; i++)
{
Vector2Int neighborPos = DirToValue(i) + cur.pos;
if (tm.HasTile(new Vector3Int(neighborPos.x, neighborPos.y, 0)) || CloseContains(neighborPos))
{
continue;
}
PFNode neighbor = Queue.Take(neighborPos);
if (neighbor == null)
{
neighbor = ScriptableObject.CreateInstance <PFNode>();
neighbor.pos = neighborPos;
neighbor.hcost = Dist(neighbor.pos, to);
}
else if (neighbor.gcost <= newGcost)
{
continue;
}
neighbor.gcost = newGcost;
neighbor.backpath = cur.pos;
if (neighbor.GetFcost() <= MaxFCost)
{
Queue.Enqueue(neighbor);
}
}
loops++;
}
PFNode endNode = Closed[Closed.Count - 1];
List <Vector2Int> BackPath = new List <Vector2Int>();
while (endNode != null)
{
BackPath.Insert(0, endNode.pos);
endNode = FindInClosed(endNode.backpath);
}
return(FinalizePositions(BackPath));
}
public NodeNotSetException(PFNode node) : base($"Node {node} not set.")
{
}
public void Enqueue(PFNode node)
{
int ind = InsertSearch(0, List.Count, node.GetFcost());
List.Insert(ind, node);
}
