Problems with my waypoints code

I have been working on a waypoint function that gives the route between two points in the form of an array and it was working fine until I added an extra waypoint. Now it ignores the waypoint when finding routes even if it is clearly part of a shorter route and if it is the goal unity crashes all together. I have looked through my code for the problem and I’m yet to find anything and I was wondering if anyone had an insight.

static var nodes = new Array ();  //will contain nodes coordinates
static var joins = new Array ();  //will contain nodes joined to each node

var cube : GameObject;  //Temporary, used as graphical indicator of points

nodes[0] = Vector3(-5,0,5);
nodes[1] = Vector3(10,0,10);
nodes[2] = Vector3(5,0,-10);
nodes[3] = Vector3(5,0,0);
nodes[4] = Vector3(-10,0,5);
nodes[5] = Vector3(10,0,5);  //Set's up the nodes coordinates
nodes[6] = Vector3(0,0,5);

joins[0] = new Array(2,3,4);
joins[1] = new Array(3,5);
joins[2] = new Array(0,5,3);
joins[3] = new Array(1,2,0);
joins[4] = new Array();
joins[4][0] = 0;
joins[5] = new Array(1,2);  //sets up nodes neighbours.
joins[6] = new Array(1,0);

function Start () 
{
	for(i=0; i<nodes.length; i++)
	{
		Instantiate(cube, nodes*, Quaternion.identity);*
  • }*
    } //Temporarry, places graphical indicators at node points

function Update ()
{

  • for(i=0; i<joins.length; i++)*
  • {*
    _ for(x=0; x<joins*.length; x++)_
    Debug.DrawLine(nodes_, nodes[joins], Color.red);
    }
    //Draws debug Lines between all neighbour nodes*

    }_

static function route(start : int, end : int)
{
* if(start==end)*
* return(Array(start,end));*
* var x=0;*
* var openNodes = new Array (); //Will contain any nodes that haven’t reached a dead end*
* var reached = new Array (); //Will contain [0]the previous node in it’s chain and [1]if it has been reached in the search*
* openNodes[0] = new Array (start, Vector3.Distance(nodes[start], nodes[end]), 0); //Sets initial openNodes value to being search at start*
* var reachedGoal=false; //Variable used to exit while loop*
* for(i=0; i<nodes.length; i++)*
* {*
_ reached = new Array(null,false);
* } //Initialises all values at null and false for the reached array*
* reached[start][1]=true;// sets the start node to be ‘reached’
while(!reachedGoal)
{
var smallest : float;
var activeNode : int;
var activeTravelled : float;
var newNode : int;
var newTravelled : float;
var activeOpen : int;
var firstCheck=true;
var newNodePrevious : int; //Various variables used inside the while loop*_

* for(i=0; i<openNodes.length; i++) //For each entry in openNodes…*
* {*
_ activeNode=openNodes*[0]; //Set the selected entry as active Node…
activeTravelled=openNodes[2]; //And the distance traveled to reach it from the start node as active travelled…
for(n=0; n<joins[activeNode].length; n++) //Then search each neighbour node…
{
var distanceCheck=activeTravelled+Vector3.Distance(nodes[activeNode],nodes[joins[activeNode][n]]); //and calculate the distance it will take to reach that node using the distance tavelled for the previous node +the distance between the two nodes*

* if((firstCheck||distanceCheck+Vector3.Distance(nodes[joins[activeNode][n]],nodes[end])<smallest)&&!reached[joins[activeNode][n]][1]) //To check if this is either the first comparison or if it is the shortest path yet, taking into account the straight line distance to the end node…
{
newNode=joins[activeNode][n];
smallest=distanceCheck;
newTravelled=distanceCheck;
firstCheck=false;
newNodePrevious=activeNode; //If it is, set it to be the new node, as well as some variables used in the next step*

* }
}
}
reached[newNode][1]=true;//Set the final new node to be ‘reached’
reached[newNode][0]=newNodePrevious;//And set which node was before it in the chain*

* openNodes[openNodes.length]=new Array(newNode, Vector3.Distance(nodes[newNode],nodes[end]), newTravelled);//Finally create a new entry for the newNode in the open Nodes array*
* if(newNode==end)
reachedGoal=true; // If the new node is the goal break the loop, if not repeat until the goal is reached*

* }
var lastNode=end; //Set a variable to hold the last checked loop in the chain(which should be the goal)
var routePath=new Array(); //Create an array to hold the path*

* routePath[0]=lastNode;//Set the first result to be the end*
* while(lastNode!=start)
{
lastNode=reached[lastNode][0];
routePath[routePath.length]=lastNode;
}//Work bacwords using the previous node information to fill the routePath with the (reversed) start to end path*

* return(routePath);//Return the path information*
}
Above is my script for the function itself and below is how I call it
* var firstCheck=true;
var endFirstCheck=true;
var shortest=0.0;
var endShortest=0.0;
for(i=0; i<waypoints.nodes.length; i++)
{
var distanceCheck=Vector3.Distance(transform.position, waypoints.nodes);
if(firstCheck||distanceCheck<shortest)
{
startNode=i;
shortest=distanceCheck;
firstCheck=false;
}
distanceCheck=Vector3.Distance(GameObject.Find(“player”).transform.position, waypoints.nodes);
if(endFirstCheck||distanceCheck<endShortest)
{
endNode=i;
endShortest=distanceCheck;
endFirstCheck=false;
}
}//Checks for the closest node and sets it as the start point*

* path=waypoints.route(startNode,endNode);// runs the routing script and sets up the pth array*_

Your while(!reachedGoal){} loop will never break if the endNode can’t be reached (for example, if node 6 is your goal) - once the openNodes list is empty, if you still haven’t been able to reach the end you should return null or something similar.