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;
}
}
