public OPNode GetClosestNode(Vector3 pos)
{
float shortestDistance = Mathf.Infinity;
OPNode node = null;
for (int i = 0; i < map.nodes.Length; i++)
{
OPNode n = map.nodes[i];
if (n == null)
{
continue;
}
float currentDistance = Vector3.Distance(pos, n.position);
if (currentDistance < 0.5)
{
node = n;
break;
}
else if (currentDistance < shortestDistance)
{
shortestDistance = currentDistance;
node = n;
}
}
return(node);
}
//private int count = 0;
public OPGridMap(Vector3 start, Vector3 size, float gridSpacing, LayerMask layerMask)
{
List <OPNode> tempList = new List <OPNode>();
spacing = gridSpacing;
int x;
int z;
// Raycast from every point in a horizontal grid
for (x = 0; x < size.x; x++)
{
for (z = 0; z < size.z; z++)
{
Vector3 from = new Vector3(start.x + (x * spacing), start.y + (size.y * spacing), start.z + (z * spacing));
RaycastHit[] hits = RaycastContinuous(from, layerMask);
// Add all hits to the list
for (int r = 0; r < hits.Length; r++)
{
Vector3 p = hits[r].point;
OPNode n = new OPNode(p.x, p.y, p.z);
tempList.Add(n);
}
}
}
nodes = tempList.ToArray();
FindNeighbors();
}
public OPNode ( Vector3 v ) {
position.x = v.x;
position.y = v.y;
position.z = v.z;
estimatedTotalCost = 0.0f;
costSoFar = 1.0f;
parent = null;
}
public OPNode ( float x, float y, float z ) {
position.x = x;
position.y = y;
position.z = z;
estimatedTotalCost = 0.0f;
costSoFar = 1.0f;
parent = null;
}
public OPNode(Vector3 v)
{
position.x = v.x;
position.y = v.y;
position.z = v.z;
estimatedTotalCost = 0.0f;
costSoFar = 1.0f;
parent = null;
}
public OPNode(float x, float y, float z)
{
position.x = x;
position.y = y;
position.z = z;
estimatedTotalCost = 0.0f;
costSoFar = 1.0f;
parent = null;
}
private void MakeNeighbors ( OPNode a, OPNode b ) {
if ( !a.neighbors.Contains ( b ) ) {
a.neighbors.Add ( b );
}
if ( !b.neighbors.Contains ( a ) ) {
b.neighbors.Add ( a );
}
}
void OnDrawGizmos()
{
if (map == null)
{
return;
}
if (map.nodes == null)
{
return;
}
Gizmos.color = Color.white;
Gizmos.DrawWireCube(bounds.center, bounds.size);
for (int i = 0; i < map.nodes.Length; i++)
{
OPNode n = map.nodes[i];
if (n == null)
{
continue;
}
Gizmos.color = new Color(0f, 0.8f, 1f, 1f);
if (n.parent != null)
{
Gizmos.color = Color.red;
}
if (n.active)
{
Gizmos.color = Color.green;
}
if (n.neighbors.Count < 1)
{
Gizmos.color = Color.red;
}
Gizmos.DrawCube(n.position, new Vector3(0.25f, 0.25f, 0.25f));
Gizmos.color = Color.green;
for (int o = 0; o < n.neighbors.Count; o++)
{
OPNode nb = n.neighbors[o];
if (n.active && nb.active)
{
Gizmos.DrawLine(n.position, nb.position);
}
}
Gizmos.color = Color.white;
}
}
private void MakeNeighbors(OPNode a, OPNode b)
{
if (!a.neighbors.Contains(b))
{
a.neighbors.Add(b);
}
if (!b.neighbors.Contains(a))
{
b.neighbors.Add(a);
}
}
/*void OnDrawGizmos()
{
Mesh mesh = this.GetComponent<MeshFilter>().sharedMesh;
List<Triangle> triangleList = new List<Triangle>();
List<OPNode> allNodes = new List<OPNode>();
int i = 0;
int nb = 0;
// Create triangles
for (i = 0; i < mesh.triangles.Length; i += 3)
{
Triangle triangle = new Triangle(
mesh.triangles[i],
mesh.triangles[i + 1],
mesh.triangles[i + 2]
);
triangleList.Add(triangle);
// Create median node
OPNode mn = new OPNode();
mn.position = this.transform.TransformPoint(triangle.GetMedianPoint(mesh));
// Add median node to list
allNodes.Add(mn);
}
Triangle[] triangleArray = triangleList.ToArray();
Vector3[] vertices = mesh.vertices;
foreach(var no in allNodes)
{
Gizmos.DrawCube(no.position, new Vector3(0.25f, 0.25f, 0.25f));
}
for (i = 0; i < triangleArray.Length; i++)
{
var n = triangleArray[i];
Gizmos.color = Color.white;
Gizmos.DrawLine(vertices[n.indices[0]], vertices[n.indices[1]]);
Gizmos.DrawLine(vertices[n.indices[1]], vertices[n.indices[2]]);
Gizmos.DrawLine(vertices[n.indices[2]], vertices[n.indices[0]]);
Gizmos.color = Color.white;
}
}*/
private void MakeNeighbors(OPNode a, OPNode b)
{
if (a == b)
return;
if (!a.neighbors.Contains(b))
a.neighbors.Add(b);
if (!b.neighbors.Contains(a))
{
b.neighbors.Add(a);
}
}
public OPNode Pop()
{
if (nodes.Count == 0)
{
return(null);
}
OPNode mn = (OPNode)nodes[0];
nodes.RemoveAt(0);
return(mn);
}
public IEnumerator FindPath(Vector3 start, Vector3 goal, List <OPNode> list)
{
if (!searching)
{
searching = true;
OPNode here = GetClosestNode(start);
OPNode there = GetClosestNode(goal);
yield return(StartCoroutine(astar.Search(here, there, map, heuristic, list, maxCycles)));
map.Reset();
searching = false;
}
}
// Locate neighbouring nodes
private void FindNeighbors()
{
for (int o = 0; o < nodes.Length; o++)
{
OPNode thisNode = nodes[o];
for (int i = 0; i < nodes.Length; i++)
{
OPNode thatNode = nodes[i];
if ((thisNode.position - thatNode.position).sqrMagnitude <= spacing * 2.1)
{
thisNode.neighbors.Add(thatNode);
}
}
}
}
public int CompareTo(System.Object obj)
{
OPNode mn = (OPNode)obj;
if (this.estimatedTotalCost < mn.estimatedTotalCost)
{
return(-1);
}
else if (this.estimatedTotalCost > mn.estimatedTotalCost)
{
return(1);
}
else
{
return(0);
}
}
public OPNode[] GetNodes()
{
Mesh mesh = this.GetComponent<MeshFilter>().sharedMesh;
List<Triangle> triangleList = new List<Triangle>();
List<OPNode> allNodes = new List<OPNode>();
int i = 0;
int nb = 0;
// Create triangles
for (i = 0; i < mesh.triangles.Length; i += 3)
{
Triangle triangle = new Triangle(
mesh.triangles[i],
mesh.triangles[i + 1],
mesh.triangles[i + 2]
);
triangleList.Add(triangle);
// Create median node
OPNode mn = new OPNode();
mn.position = this.transform.TransformPoint(triangle.GetMedianPoint(mesh));
// Add median node to list
allNodes.Add(mn);
}
Triangle[] triangleArray = triangleList.ToArray();
Vector3[] vertices = mesh.vertices;
// Connect median nodes
for (i = 0; i < triangleArray.Length; i++)
{
var gn = triangleArray[i].GetNeighbors(triangleArray, vertices);
for (nb = 0; nb < gn.Count; nb++)
{
MakeNeighbors(allNodes[i], allNodes[gn[nb]]);
}
}
// Return
return allNodes.ToArray();
}
public void SetGoal(Vector3 v, bool persist)
{
if (goal == v)
{
return;
}
var gNode = scanner.GetClosestNode(v);
if (gNode == goalNode)
{
return;
}
goal = v;
goalNode = gNode;
UpdatePosition(persist);
}
// Helper void used to build path for AStar search
private void GetPath(OPNode node, List <OPNode> list)
{
// Traverse the path from goal to start
int counter = 0;
while (node != null)
{
if (counter > 100)
{
Debug.LogError("OpenPath | Screech! Failsafe engaged.");
return;
}
list.Add(node);
node = node.parent;
counter++;
}
// Reverse it
list.Reverse();
}
public OPNode[] GetNodes () {
Mesh mesh = this.GetComponent<MeshFilter>().sharedMesh;
List< Triangle > triangleList = new List< Triangle > ();
List< OPNode > allNodes = new List< OPNode > ();
int i = 0;
int nb = 0;
// Create triangles
for ( i = 0; i < mesh.triangles.Length; i += 3 ) {
Triangle triangle = new Triangle (
mesh.triangles [ i ],
mesh.triangles [ i + 1 ],
mesh.triangles [ i + 2 ]
);
triangleList.Add ( triangle );
// Create median node
OPNode mn = new OPNode ();
mn.position = this.transform.TransformPoint ( triangle.GetMedianPoint ( mesh ) );
// Add median node to list
allNodes.Add ( mn );
}
Triangle[] triangleArray = triangleList.ToArray();
Vector3[] vertices = mesh.vertices;
// Connect median nodes
for ( i = 0; i < triangleArray.Length; i++ ) {
for ( nb = 0; nb < triangleArray[i].GetNeighbors ( triangleArray, vertices ).Count; nb++ ) {
MakeNeighbors ( allNodes [ i ], allNodes [ nb ] );
}
}
// Return
return allNodes.ToArray();
}
//private int count = 0;
public OPGridMap ( Vector3 start, Vector3 size, float gridSpacing, LayerMask layerMask ) {
List< OPNode > tempList = new List< OPNode >();
spacing = gridSpacing;
int x;
int z;
// Raycast from every point in a horizontal grid
for ( x = 0; x < size.x; x++ ) {
for ( z = 0; z < size.z; z++ ) {
Vector3 from = new Vector3 ( start.x + (x*spacing), start.y + (size.y*spacing), start.z + (z*spacing) );
RaycastHit[] hits = RaycastContinuous ( from, layerMask );
// Add all hits to the list
for ( int r = 0; r < hits.Length; r++ ) {
Vector3 p = hits[r].point;
OPNode n = new OPNode ( p.x, p.y, p.z );
tempList.Add ( n );
}
}
}
nodes = tempList.ToArray();
FindNeighbors ();
}
public List<OPNode> GetNeighbors ( OPNode node ) {
return node.neighbors;
}
public OPNode()
{
estimatedTotalCost = 0.0f;
costSoFar = 1.0f;
parent = null;
}
public int Push ( OPNode node ) {
nodes.Add ( node );
nodes.Sort();
return nodes.Count;
}
// Find a path and return a list of each step
public IEnumerator Search ( OPNode start, OPNode goal, OPMap map, float heuristicWeight, List< OPNode > list, int maxCycles ) {
Debug.Log(start.ToString() + ", " + goal.ToString());
if ( start != null && goal != null ) {
// Add the starting node to the open list
openList = new OPPriorityQueue ();
openList.Push ( start );
start.costSoFar = 0;
start.estimatedTotalCost = HeuristicEstimate ( start, goal, heuristicWeight );
closedList = new OPPriorityQueue ();
OPNode currentNode = null;
int cycles = 0;
// While the open list is not empty
while ( openList.GetLength() != 0 ) {
// Current node = node from the open list with the lowest cost
currentNode = openList.Front();
if ( currentNode == goal ) {
break;
}
// Examine each node adjacent to the current node
List <OPNode> neighbors = map.GetNeighbors ( currentNode );
for ( int nIndex = 0; nIndex != neighbors.Count; nIndex++ ) {
// Get the cost estimate for the end node
OPNode endNode = (OPNode) neighbors[nIndex];
float incrementalCost = GetCost ( currentNode, endNode );
float endNodeCost = currentNode.costSoFar + incrementalCost;
// If the node is closed we may have to skip or remove it from the closed list.
if ( closedList.Contains ( endNode ) ) {
// If we didn't find a shorter route, skip.
if ( endNode.costSoFar <= endNodeCost ) {
continue;
}
// Otherwise remove it from the closed list
closedList.Remove( endNode );
// Skip if the node is open and we haven't found a better route
} else if ( openList.Contains ( endNode ) ) {
// If our route is no better, then skip
if(endNode.costSoFar <= endNodeCost ) {
continue;
}
}
float endNodeHeuristic = HeuristicEstimate ( endNode, goal, heuristicWeight );
// We are here if we need to update the node
// Update the cost and estimate
endNode.costSoFar = endNodeCost;
endNode.parent = currentNode;
endNode.estimatedTotalCost = endNodeCost + endNodeHeuristic;
// And add it to the open list
if ( !openList.Contains ( endNode ) ) {
openList.Push(endNode);
}
}
// We've finished looking at the neighbors for the current node, so add it to the closed list and remove it from the open list
closedList.Push ( currentNode );
openList.Remove ( currentNode );
if ( cycles > maxCycles ) {
cycles = 0;
yield return null;
} else {
cycles++;
}
}
if ( currentNode.Equals ( goal ) ) {
// Path complete
GetPath ( currentNode, list );
}
}
}
// Helper void used to build path for AStar search
private void GetPath ( OPNode node, List< OPNode > list ) {
// Traverse the path from goal to start
int counter = 0;
while ( node != null ) {
if ( counter > 100 ) {
Debug.LogError ( "OpenPath | Screech! Failsafe engaged." );
return;
}
list.Add ( node );
node = node.parent;
counter++;
}
// Reverse it
list.Reverse();
}
public int Push(OPNode node)
{
nodes.Add(node);
nodes.Sort();
return(nodes.Count);
}
public int GetIndex(OPNode node)
{
return(System.Array.IndexOf(nodes, node));
}
private float GetCost(OPNode node0, OPNode node1)
{
return((node0.position - node1.position).magnitude);
}
// Find a path and return a list of each step
public IEnumerator Search(OPNode start, OPNode goal, OPMap map, float heuristicWeight, List <OPNode> list, int maxCycles)
{
Debug.Log(start.position + ", " + goal.position);
if (start != null && goal != null)
{
// Add the starting node to the open list
openList = new OPPriorityQueue();
openList.Push(start);
start.costSoFar = 0;
start.estimatedTotalCost = HeuristicEstimate(start, goal, heuristicWeight);
closedList = new OPPriorityQueue();
OPNode currentNode = null;
int cycles = 0;
// While the open list is not empty
while (openList.GetLength() != 0)
{
// Current node = node from the open list with the lowest cost
currentNode = openList.Front();
if (currentNode == goal)
{
break;
}
// Examine each node adjacent to the current node
List <OPNode> neighbors = map.GetNeighbors(currentNode);
for (int nIndex = 0; nIndex != neighbors.Count; nIndex++)
{
// Get the cost estimate for the end node
OPNode endNode = (OPNode)neighbors[nIndex];
float incrementalCost = GetCost(currentNode, endNode);
float endNodeCost = currentNode.costSoFar + incrementalCost;
// If the node is closed we may have to skip or remove it from the closed list.
if (closedList.Contains(endNode))
{
// If we didn't find a shorter route, skip.
if (endNode.costSoFar <= endNodeCost)
{
continue;
}
// Otherwise remove it from the closed list
closedList.Remove(endNode);
// Skip if the node is open and we haven't found a better route
}
else if (openList.Contains(endNode))
{
// If our route is no better, then skip
if (endNode.costSoFar <= endNodeCost)
{
continue;
}
}
float endNodeHeuristic = HeuristicEstimate(endNode, goal, heuristicWeight);
// We are here if we need to update the node
// Update the cost and estimate
endNode.costSoFar = endNodeCost;
endNode.parent = currentNode;
endNode.estimatedTotalCost = endNodeCost + endNodeHeuristic;
// And add it to the open list
if (!openList.Contains(endNode))
{
openList.Push(endNode);
}
}
// We've finished looking at the neighbors for the current node, so add it to the closed list and remove it from the open list
closedList.Push(currentNode);
openList.Remove(currentNode);
if (cycles > maxCycles)
{
cycles = 0;
yield return(null);
}
else
{
cycles++;
}
}
if (currentNode.Equals(goal))
{
// Path complete
GetPath(currentNode, list);
}
}
}
private float HeuristicEstimate(OPNode currNode, OPNode goal, float heuristicWeight)
{
return((currNode.position - goal.position).magnitude * heuristicWeight);
}
public bool Contains(OPNode node)
{
return(nodes.Contains(node));
}
public void Remove(OPNode node)
{
nodes.Remove(node);
nodes.Sort();
}
public int GetIndex ( OPNode node ) {
return System.Array.IndexOf ( nodes, node );
}
public void SetMap ( OPNode[] nodes ) {
map = new OPMap ();
map.nodes = nodes;
}
private float HeuristicEstimate ( OPNode currNode, OPNode goal, float heuristicWeight ) {
return ( currNode.position - goal.position ).magnitude * heuristicWeight;
}
public void Remove ( OPNode node ) {
nodes.Remove ( node );
nodes.Sort();
}
public List <OPNode> GetNeighbors(OPNode node)
{
return(node.neighbors);
}
public bool Contains ( OPNode node ) {
return nodes.Contains ( node );
}
private float GetCost ( OPNode node0, OPNode node1 ) {
return ( node0.position - node1.position ).magnitude;
}
public OPNode () {
estimatedTotalCost = 0.0f;
costSoFar = 1.0f;
parent = null;
}
