// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
public bool CheckForNodeInParentChain(TraversableNode targetNode)
{
if (this == targetNode)
{
return(true);
}
ValidateParentChain(this);
TraversableNode tNode = this;
while (tNode.parentNode != null && tNode.parentNode != this)
{
if (tNode.parentNode == targetNode)
{
return(true);
}
else
{
tNode = tNode.parentNode;
}
}
return(false);
}
public TraversableNode(Vector3 _pos, Vector3 origin, Vector3 target, TraversableNode _previousNode)
{
g = Utils.GetASharpDistance(_pos, origin);
h = Utils.GetASharpDistance(_pos, target);
previousNode = _previousNode;
pos = new Vector3(_pos.x, 0, _pos.z);
f = (_pos - target).sqrMagnitude;
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
private void VeggieJump()
{
if (hasPath)
{
TraversableNode nextNode = _nodePathStack.Peek();
float dist = Vector3.Distance(transform.position, nextNode.transform.position);
Vector3 nextPos = nextNode.transform.up * (Mathf.Sin(dist * Mathf.PI));
transform.GetChild(0).transform.localPosition = Vector3.up * 0.2f + nextPos;
}
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
public void SetRandomDestination(int min, int range)
{
if (currentPositionNode == null)
{
return;
}
Node[] neighbors = currentPositionNode.GetNeighborhoodLayers(min, range);
TraversableNode destNode = neighbors[Random.Range(0, neighbors.Length)] as TraversableNode;
goalPositionNode = destNode;
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
public static void ReverseParents(TraversableNode currentNode)
{
TraversableNode previousNode = null;
TraversableNode nextNode = null;
do
{
nextNode = currentNode.parentNode;
currentNode.parentNode = previousNode;
previousNode = currentNode;
currentNode = nextNode;
} while(currentNode != null);
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
private static int AddToSortedList(TraversableNode node, ref List <TraversableNode> sortedList)
{
for (int i = 0; i < sortedList.Count; i++)
{
if (node < sortedList[i])
{
sortedList.Insert(i, node);
return(i);
}
}
sortedList.Add(node);
return(sortedList.Count);
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
private static Stack <TraversableNode> NodePathStack(TraversableNode endNode)
{
Stack <TraversableNode> returnStack = new Stack <TraversableNode>();
TraversableNode currentNode = endNode;
TraversableNode.ValidateParentChain(currentNode);
while (currentNode.parentNode != null)
{
returnStack.Push(currentNode);
currentNode = currentNode.parentNode;
}
return(returnStack);
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
public static float Distance(TraversableNode aNode, TraversableNode bNode)
{
float a = (aNode.xCoord - bNode.xCoord);
float b = (aNode.yCoord - bNode.yCoord);
a *= a;
b *= b;
return(Mathf.Sqrt(a + b));
// float a = Mathf.Abs(aNode.xCoord - bNode.xCoord);
// float b = Mathf.Abs(aNode.yCoord - bNode.yCoord);
// return (a + b);
}
public IEnumerator NewRandomPathRoutine()
{
yield return(new WaitForSeconds(Random.Range(0f, waitTime)));
foreach (Node na in navAgent.currentPositionNode.GetNeighborhood(3))
{
NodeNavAgent[] agentsInRange = na.GetInformation <NodeNavAgent>();
if (agentsInRange.Length > 0)
{
Node[] potentialTargets = agentsInRange[0].currentPositionNode.GetNeighborhoodLayers(2, 1);
TraversableNode goal = potentialTargets[Random.Range(0, potentialTargets.Length)] as TraversableNode;
navAgent.goalPositionNode = goal;
Debug.DrawLine(navAgent.currentPositionNode.transform.position, navAgent.goalPositionNode.transform.position, Color.red, 10f, false);
}
}
navAgent.SetRandomDestination(2, 2);
}
private void gizmosTraversabilityMapTraversableNodesAndEdges(Analyzer a)
{
foreach (TraversableNode x in a.getTraversabilityMap())
{
if (x.isTraversable())
{
Gizmos.DrawSphere(x.getPosition(), 0.5f);
foreach (Direction dir in System.Enum.GetValues(typeof(Direction)))
{
TraversableNode temp = x.getNeighbour(dir);
if (temp != null && temp.isTraversable())
{
Gizmos.DrawLine(temp.getPosition(), x.getPosition());
}
}
}
}
}
public IEnumerator NewRandomPathRoutine()
{
yield return(new WaitForSeconds(Random.Range(0f, waitTime)));
foreach (Node n in navAgent.currentPositionNode.nodeData.GetNeighborhood(3))
{
NodeNavAgent[] agentsInRange = n.GetInformation <NodeNavAgent>();
if (agentsInRange.Length > 0 && agentsInRange[0].payload is Cow leader)
{
followedAgent = leader;
TraversableNode[] potentialTargets = followedAgent.navAgent.currentPositionNode.nodeData.GetNeighborhoodLayersInformation <TraversableNode>(1, 1);
int index = Random.Range(0, potentialTargets.Length - 1);
TraversableNode goal = potentialTargets[index];
navAgent.goalPositionNode = goal;
yield break;
}
}
navAgent.SetRandomDestination(2, 2);
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
public static void ValidateParentChain(TraversableNode aNode)
{
List <TraversableNode> parents = new List <TraversableNode>();
while (aNode.parentNode != null)
{
if (parents.Contains(aNode.parentNode))
{
aNode.parentNode = null;
return;
}
else
{
parents.Add(aNode.parentNode);
aNode = aNode.parentNode;
}
}
}
// Update is called once per frame
void Update()
{
if (Input.GetMouseButtonDown(0))
{
Ray cameraRay = mainCamera.ScreenPointToRay(Input.mousePosition);
RaycastHit rayHit;
if (Physics.Raycast(cameraRay, out rayHit))
{
TraversableNode tn = rayHit.collider.GetComponent <TraversableNode>();
if (tn == null)
{
return;
}
else
{
GameObject go = Instantiate(cow, tn.transform.position, Quaternion.identity);
go.GetComponent <NodeNavAgent>().currentPositionNode = tn;
}
}
}
}
private void ClickSetPath()
{
RaycastHit hit;
if (Physics.Raycast(Camera.main.ScreenPointToRay(Input.mousePosition), out hit))
{
TraversableNode tn = hit.collider.GetComponent <TraversableNode>();
if (tn == null)
{
return;
}
else if (Input.GetMouseButtonDown(1))
{
if (_startNode != null)
{
_startNode.ResetMaterial();
}
path = null;
_startNode = tn;
_startNode.GetComponent <Renderer>().material = mat;
transform.position = _startNode.transform.position;
}
else if (Input.GetMouseButtonDown(0))
{
_endNode = tn;
_endNode.GetComponent <Renderer>().material = mat;
transform.position = _startNode.transform.position;
if (_startNode != null && _endNode != null)
{
path = NodeNav.TwinStarII(_startNode, _endNode, true);
}
}
}
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
private void ClickSetPath()
{
RaycastHit hit;
if (Physics.Raycast(Camera.main.ScreenPointToRay(Input.mousePosition), out hit))
{
TraversableNode tn = hit.collider.GetComponent <TraversableNode>();
if (tn == null)
{
return;
}
else if (Input.GetMouseButtonDown(1))
{
if (currentPositionNode != null)
{
currentPositionNode.isOccupied = false;
currentPositionNode.RemoveInformation(this);
}
_nodePathStack = null;
_currentPositionNode = tn;
currentPositionNode.isOccupied = true;
currentPositionNode.AddInformation(this);
transform.position = currentPositionNode.transform.position;
}
else if (Input.GetMouseButtonDown(0))
{
if (currentPositionNode != null && tn != null)
{
_nodePathStack = NodeNav.TwinStarII(currentPositionNode, tn, true);
}
}
}
}
private void WalkPath()
{
if (path != null && path.Count > 0)
{
_startNode = path.Peek();
Vector3 dir = (path.Peek().transform.position - transform.position);
transform.GetChild(0).transform.localPosition = Vector3.up + (path.Peek().transform.up *Mathf.Sin(dir.magnitude * 3.14f));
dir.Normalize();
transform.Translate(dir * 2.5f * Time.deltaTime);
if (Vector3.Distance(transform.position, path.Peek().transform.position) <= 0.05f)
{
path.Pop().GetComponent <Renderer>().material = mat;
}
if (path.Count == 0)
{
OnPathComplete.Invoke();
path = null;
}
}
}
public static List <TraversableNode> GetASharpPath(Vector3 origin, Vector3 target, Material pathMaterial)
{
Debug.Log("Finding A* path from : " + origin.ToString() + " to " + target.ToString());
target = new Vector3(Mathf.Floor(target.x), 0, Mathf.Floor(target.z));
origin = new Vector3(Mathf.Floor(origin.x), 0, Mathf.Floor(origin.z));
List <TraversableNode> openNodes = new List <TraversableNode> ();
List <TraversableNode> closedNodes = new List <TraversableNode> ();
TraversableNode originNode = new TraversableNode(origin, origin, target, null);
TraversableNode currentNode;
bool pathFound = false;
openNodes.Add(originNode);
do
{
currentNode = openNodes.OrderBy(x => x.f).FirstOrDefault();
//Debug.Log("Current node coordinates " + currentNode.pos.ToString());
closedNodes.Add(currentNode);
openNodes.Remove(currentNode);
if (closedNodes.Where(x => x.pos == target).FirstOrDefault() != null)
{
pathFound = true;
//Debug.Log(closedNodes.Where(x => x.pos == target).First().pos.ToString());
break;
}
List <TraversableNode> tempNodes = new List <TraversableNode>();
tempNodes.Add(new TraversableNode(new Vector3(currentNode.pos.x, 0, currentNode.pos.z + 1), origin, target, currentNode));
tempNodes.Add(new TraversableNode(new Vector3(currentNode.pos.x, 0, currentNode.pos.z - 1), origin, target, currentNode));
tempNodes.Add(new TraversableNode(new Vector3(currentNode.pos.x - 1, 0, currentNode.pos.z), origin, target, currentNode));
tempNodes.Add(new TraversableNode(new Vector3(currentNode.pos.x + 1, 0, currentNode.pos.z), origin, target, currentNode));
foreach (TraversableNode tv in tempNodes)
{
GameObject tile;
if (tile = GetTileFromTransformPosition(tv.pos))
{
if (tile.transform.childCount == 0)
{
TraversableNode newNode = new TraversableNode(tile.transform.position, origin, target, currentNode);
//Debug.Log("Nodes in openlist : " + openNodes.Count);
//Debug.Log("Checking tile at position : " + tile.transform.position.ToString());
//Debug.Log("newNode position = " + newNode.pos.ToString());
if (openNodes.Where(x => x.pos == newNode.pos).FirstOrDefault() == null)
{
if (closedNodes.Where(x => x.pos == newNode.pos).FirstOrDefault() == null)
{
//Debug.Log("Adding node : " + newNode.pos.ToString() + " with f value of " + newNode.f.ToString());
openNodes.Add(newNode);
}
else
{
//Debug.Log("Found in closed List");
}
}
else
{
//Debug.Log("Found in open List");
}
}
}
}
//Debug.Log("..................................");
} while(openNodes.Count > 0);
List <TraversableNode> path = new List <TraversableNode> ();
while (currentNode.previousNode != null)
{
path.Add(currentNode);
currentNode = currentNode.previousNode;
GameObject tile = GetTileFromTransformPosition(currentNode.pos);
tile.GetComponent <Renderer> ().material = pathMaterial;
}
if (path.Count > 0)
{
if (pathFound)
{
return(path);
}
else
{
Debug.Log("Go berserk");
}
}
else
{
Debug.Log("Go berserk");
}
return(null);
}
// traversabilityMap methods
private void makeTraversabilityMap()
{
traversabilityGrid = new TraversableNode[grid.GetLength(0), grid.GetLength(1)];
for (int x = 0; x < grid.GetLength(0); x++)
{
for (int y = 0; y < grid.GetLength(1); y++)
{
if (grid[x, y] >= maxReachableHeight)
{
}
else
{
TraversableNode toAdd = new TraversableNode(new Vector3(gridIntToWorldfloatX(x), grid[x, y], gridIntToWorldfloatY(y)), new GridCoordinate(x, y), true);
traversabilityMap.Add(toAdd);
traversabilityGrid[x, y] = toAdd;
}
}
}
foreach (TraversableNode node in traversabilityMap)
{
GridCoordinate nodeCoordinate = node.getCoordinate();
foreach (Direction direction in System.Enum.GetValues(typeof(Direction)))
{
GridCoordinate directionCoordinate = DirectionMethods.directionToGrid(direction);
if (nodeCoordinate.x + directionCoordinate.x >= grid.GetLength(0) || nodeCoordinate.x + directionCoordinate.x < 0 || nodeCoordinate.y + directionCoordinate.y >= grid.GetLength(1) || nodeCoordinate.y + directionCoordinate.y < 0)
{
continue;
}
if (grid[nodeCoordinate.x + directionCoordinate.x, nodeCoordinate.y + directionCoordinate.y] < grid[nodeCoordinate.x, nodeCoordinate.y] + maxTraversableSlope)
{
node.insertAt(direction, traversabilityGrid[nodeCoordinate.x + directionCoordinate.x, nodeCoordinate.y + directionCoordinate.y]);
}
else
{
TraversableNode tempNode = traversabilityGrid[nodeCoordinate.x + directionCoordinate.x, nodeCoordinate.y + directionCoordinate.y];
if (tempNode != null)
{
tempNode.setTraversable(false);
node.insertAt(direction, tempNode);
}
else
{
node.insertAt(direction, TraversableNode.getEmptyNode());
}
}
}
}
List <TraversableNode> tempList = new List <TraversableNode>();
foreach (TraversableNode node in traversabilityMap)
{
if (!node.isTraversable())
{
tempList.Add(node);
}
}
foreach (TraversableNode node in tempList)
{
traversabilityMap.Remove(node);
}
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
public static Stack <TraversableNode> TwinStarII(TraversableNode begNode, TraversableNode endNode, bool dualSearch = false)
{
if (begNode == endNode || begNode == null || endNode == null || !endNode.isTraversable)
{
return(null);
}
List <TraversableNode>[] openList = new List <TraversableNode>[] { new List <TraversableNode>(), new List <TraversableNode>() };
List <TraversableNode> closedList = new List <TraversableNode>();
TraversableNode[] currentNode = new TraversableNode[] { begNode, endNode };
begNode.parentNode = endNode.parentNode = null;
openList[0].Add(currentNode[0]);
openList[1].Add(currentNode[1]);
// As long as there are nodes to check
while (openList[0].Count > 0 || openList[1].Count > 0)
{
// If dualSearch is enabled, the we will check from the start and end node until the 2 meet
for (int i = 0; i < 2; i += dualSearch ? 1 : 0)
{
// For each of the neighbor nodes of our current node
foreach (TraversableNode neighborNode in currentNode[i].GetNeighbors())
{
// If the neighbor cannot be traversed,
// move to the next one
if (neighborNode == null || neighborNode.isTraversable == false)
{
continue;
}
// If it CAN be traversed AND it's root parent is the goal node,
// return the path
else if (i == 0 && neighborNode.CheckForNodeInParentChain(endNode))
{
TraversableNode.ReverseParents(neighborNode);
neighborNode.parentNode = currentNode[0];
return(NodePathStack(endNode));
}
// Else, the node IS traversable, and NOT connected to the goal node
else
{
// Check if the node has already been traversed, or is on the list to be checked,
// and add it to the list if it needs to be
if (!closedList.Contains(neighborNode))
{
if (!openList[i].Contains(neighborNode))
{
neighborNode.parentNode = currentNode[i];
neighborNode.hValue = TraversableNode.Distance(neighborNode, endNode);
neighborNode.gValue = neighborNode.GetGValue(i == 1);
AddToSortedList(neighborNode, ref openList[i]);
}
}
// If the neighbor's G value is less than the parent's,
// reparent the current node to the neighbor
else if (neighborNode.safeParentNode != currentNode[i] &&
neighborNode.gValue < currentNode[i].safeParentNode.gValue)
{
currentNode[i].parentNode = neighborNode;
}
}
}
AddToSortedList(currentNode[i], ref closedList);
currentNode[i] = openList[i][0];
openList[i].Remove(currentNode[i]);
}
}
// A path could not be found, so return an empty path stack
return(NodePathStack(begNode));
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
public float GetNeighboorTravelCost(TraversableNode aNode, bool invert = false)
{
return(!invert?
CheckIsNeighbor(aNode) ? (this.travelCost - aNode.travelCost) : float.MaxValue:
aNode.GetNeighboorTravelCost(this, false));
}
// ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------
private static TraversableNode GetRootParent(TraversableNode aNode)
{
return(aNode.parentNode == null ? aNode : GetRootParent(aNode.parentNode));
}
//Insert a Traversable node in the direction slot this node can get to it by moving
public void insertAt(Direction direction, TraversableNode node)
{
movableNodes[DirectionMethods.directionToArrayIndex(direction)] = node;
}