public List<Vector3> Path(Vector3 startPos, Vector3 targetPos) { //Can I see the target float targetDistance = Vector3.Distance(startPos, targetPos); //if the target and start are the same then just return path with the two nodes if (!Physics.Raycast(startPos, targetPos - startPos, targetDistance)) { List<Vector3> path = new List<Vector3>(); path.Add(startPos); path.Add(targetPos); return path; } //find all the nodes with the specified nodeTag GameObject[] nodes = GameObject.FindGameObjectsWithTag(nodeTag); //create new class list List<Point> path_points = new List<Point>(); foreach (GameObject n in nodes) { Point currNode = new Point(n.transform.position); path_points.Add(currNode); } Point endPoint = new Point(targetPos, "end"); foreach(Point p in path_points) { foreach (Point p2 in path_points) { float distance = Vector3.Distance(p.GetPos(), p2.GetPos()); if (!Physics.Raycast(p.GetPos(), p2.GetPos() - p.GetPos(), distance)) { p.AddConnectedPoint(p2); } } float distance2 = Vector3.Distance(targetPos, p.GetPos()); if (!Physics.Raycast(targetPos, p.GetPos() - targetPos, distance2)) { p.AddConnectedPoint(endPoint); } } //path_points startPos can see foreach (Point p in path_points) { float distance = Vector3.Distance(startPos, p.GetPos()); if (!Physics.Raycast(startPos, p.GetPos() - startPos, distance)) { Point startPoint = new Point(startPos, "start"); p.SetPrevPoint(startPoint); p.SetState("nb"); p.SetScore(distance + Vector3.Distance(targetPos, p.GetPos())); } } //Go through until we find the exit or run out of connections bool searchedAll = false; bool targetFound = false; while(!searchedAll) { searchedAll = true; List<Point> foundConnections = new List<Point>(); foreach (Point point in path_points) { if (point.GetState() == "nb") { searchedAll = false; List<Point> potentials = point.GetConnectedpath_points(); foreach (Point potentialPoint in potentials) { if (potentialPoint.GetState() == "unset") { potentialPoint.AddPotentialPrevPoint(point); foundConnections.Add(potentialPoint); potentialPoint.SetScore(Vector3.Distance(startPos, potentialPoint.GetPos()) + Vector3.Distance(targetPos, potentialPoint.GetPos())); } else if (potentialPoint.GetState() == "end") { //Found the exit targetFound = true; endPoint.AddConnectedPoint(point); } } point.SetState("useless"); } } foreach (Point c in foundConnections) { c.SetState("nb"); float lowestScore = 0; Point bestPrevPoint = null; bool first = true; foreach (Point prevpath_points in c.GetPotentialPrevpath_points()) { if (first) { lowestScore = prevpath_points.GetScore(); bestPrevPoint = prevpath_points; first = false; } else { if (lowestScore > prevpath_points.GetScore()) { lowestScore = prevpath_points.GetScore(); bestPrevPoint = prevpath_points; } } } c.SetPrevPoint(bestPrevPoint); } } if (targetFound) { List<Point> bestPath = null; float lowestScore = 0; bool firstRoute = true; foreach (Point p in endPoint.GetConnectedpath_points()) { float score = 0; bool finding = true; Point c_Point = p; List<Point> route = new List<Point>(); route.Add(endPoint); while(finding) { route.Add(c_Point); if (c_Point.GetState() == "start") { if (firstRoute) { bestPath = route; lowestScore = score; firstRoute = false; } else { if (lowestScore > score) { bestPath = route; lowestScore = score; } } finding = false; break; } score += c_Point.GetScore(); c_Point = c_Point.GetPrevPoint(); } } bestPath.Reverse(); List<Vector3> path = new List<Vector3>(); foreach (Point p in bestPath) { path.Add(p.GetPos()); } return path; } else { return null; } }