public List <Informer> JPS(Informer from, Informer to)
{
DebugInformationAlgorithm debugInformation;
var finalPath = JPS(from, to, false, out debugInformation);
return(Extensions.ToInformers(finalPath));
}
public List <Informer> AStar(Informer from, Informer to, float radius)
{
DebugInformationAlgorithm debugInformation;
var finalPath = AStar(from, to, radius, false, out debugInformation);
return(finalPath);
}
public void RegisterInformer(Informer informer)
{
var position = informer.transform.position;
NodesTree.Add(position.ToArray(), informer);
NodesArray [(int)position.x / 3, (int)position.z / 3] = new Node(informer, NodeState.Undiscovered);
}
public Node(Informer i, NodeState v)
{
InformerNode = i;
Visited = v;
Distance = 0;
Position = i.transform.position;
DestinationFromStart = Destinations.Default;
DestinationFromPrevious = Destinations.Default;
DestinationToFinish = Destinations.Default;
Iteration = 0;
for (var k = 0; k < 3; ++k)
{
for (var j = 0; j < 3; ++j)
{
NormMatrix[k, j] = 0;
}
}
for (var dir = 0; dir < 8; ++dir)
{
GoalBounds[dir, (int)GoalBoundsEnum.MinRow] = 1000;
GoalBounds[dir, (int)GoalBoundsEnum.MaxRow] = 0;
GoalBounds[dir, (int)GoalBoundsEnum.MinCol] = 1000;
GoalBounds[dir, (int)GoalBoundsEnum.MaxCol] = 0;
}
IsJumpPoint = JPType.Default;
TargetJP = false;
}
public List <Node> JPS(Informer from, Informer to, bool debugFlag, out DebugInformationAlgorithm debugInformation)
{
if (from == null || to == null)
{
Debug.LogError("Can't run JPS+. Enter proper from and to parameters!");
debugInformation = null;
return(null);
}
}
public Node(Node n)
{
if (n != null)
{
InformerNode = n.InformerNode;
Visited = n.Visited;
Distance = n.Distance;
Position = n.Position;
DestinationToFinish = n.DestinationToFinish;
DestinationFromPrevious = n.DestinationFromPrevious;
NormMatrix = n.NormMatrix;
IsJumpPoint = n.IsJumpPoint;
TargetJP = n.TargetJP;
}
}
public Node(Informer i, NodeState v)
{
InformerNode = i;
Visited = v;
Distance = 0;
Position = i.transform.position;
DestinationFromPrevious = Destinations.Default;
DestinationToFinish = Destinations.Default;
for (var k = 0; k < 3; ++k)
{
for (var j = 0; j < 3; ++j)
{
NormMatrix[k, j] = 0;
}
}
IsJumpPoint = JPType.Default;
TargetJP = false;
}
public List <Informer> AStar(Informer from, Informer to, float radius, bool debugFlag,
out DebugInformationAlgorithm debugInformation)
{
if (from == null || to == null)
{
Debug.LogError("Can't run A*. Enter proper from and to parameters!");
debugInformation = null;
return(null);
}
//Debug.Log("From: " + from.transform.position);
//Debug.Log("To: " + to.transform.position);
if (debugFlag)
{
debugInformation = new DebugInformationAlgorithm
{
From = from,
To = to,
Observed = new List <Node>(),
FinalPath = new List <Informer>(),
LinesFromFinish = new List <Node>(),
CrossPoints = new List <Node>()
};
}
else
{
debugInformation = null;
}
var current = new Node(from, NodeState.Processed);
current.Distance = current.InformerNode.MetricsAStar(to);
var observed = new List <Node> {
current
};
// ReSharper disable once PossibleNullReferenceException
while (current.InformerNode != to)
{
var query = NodesTree.Nearest(current.InformerNode.transform.position.ToArray(), radius).ToList();
query =
query.Where(
informer => informer.InformerNode.transform.position != current.InformerNode.transform.position &&
informer.InformerNode.IsObstacle != true)
.ToList();
foreach (var informer in query)
{
if (
!observed.Exists(
arg => arg.InformerNode.transform.position == informer.InformerNode.transform.position))
{
informer.Distance = informer.InformerNode.MetricsAStar(to);
informer.Visited = NodeState.Discovered;
observed.Add(informer);
}
}
observed = observed.OrderBy(arg => arg.Visited).ThenBy(arg => arg.Distance).ToList();
if (observed[0].Visited != NodeState.Processed)
{
current = observed[0];
observed[0].Visited = NodeState.Processed;
if (debugInformation != null)
{
debugInformation.Observed.Add(observed[0]);
}
}
else
{
Debug.Log("No path was found");
debugInformation = null;
return(null);
}
}
observed = observed.Where(informer => informer.Visited == NodeState.Processed).ToList();
var finalPath = new List <Informer>();
var path = new List <Node> {
current
};
while (current.InformerNode != from)
{
var temp = current;
var tempFrom = temp.InformerNode.MetricsAStar(from);
var flag = false;
foreach (var informer in observed)
{
if (informer.InformerNode.MetricsAStar(current.InformerNode) < 18.1 &&
informer.Visited == NodeState.Processed)
{
var informerFrom = informer.InformerNode.MetricsAStar(from);
if (tempFrom > informerFrom ||
tempFrom <= informerFrom && flag == false)
{
if (flag)
{
observed.Find(arg => arg.InformerNode.transform.position
== temp.InformerNode.transform.position).Visited =
NodeState.Processed;
}
informer.Visited = NodeState.Undiscovered;
temp = informer;
tempFrom = temp.InformerNode.MetricsAStar(from);
flag = true;
}
}
}
if (!flag)
{
path.RemoveAt(path.Count - 1);
current = path[path.Count - 1];
}
else
{
path.Add(temp);
current = temp;
}
}
Debug.Log("Path: " + path.Count);
finalPath.Add(path[0].InformerNode);
for (var i = 1; i < path.Count; ++i)
{
var maxIndex = i;
for (var j = i; j < path.Count; ++j)
{
if (StraightLine.OnOneLine(path[i], path[j], NodesArray))
{
maxIndex = j;
}
}
if (maxIndex != i)
{
var points = StraightLine.FindMiddlePoints(path[i], path[maxIndex]);
foreach (var point in points)
{
finalPath.Add(NodesArray[point.X, point.Y].InformerNode);
}
i = maxIndex;
}
}
finalPath.Reverse();
Debug.Log("Final path: " + finalPath.Count);
/*var loopflag = false;
* Node loopstart = null;
* for (var i = path.Count - 1; i >= 0; --i) {
* var neighbours = NodesTree.Nearest(path[i].InformerNode.transform.position.ToArray(), radius).ToList();
* var intersection = 0;
* foreach (var informer in neighbours) {
* if (
* path.Exists(
* arg => arg.InformerNode.transform.position == informer.InformerNode.transform.position))
++intersection;
* }
* if (intersection > 3) {
* if (!loopflag) {
* loopflag = true;
* int index;
* if (i < path.Count - 1) {
* index = i + 1;
* } else {
* index = i;
* }
* loopstart = path[index];
* finalPath.Remove( loopstart.InformerNode );
* //Debug.Log("Loopstart: " + loopstart.InformerNode.transform.position);
* }
* } else {
* int index;
* if (i > 0) {
* index = i - 1;
* } else {
* index = i;
* }
* intersection = 0;
* neighbours = NodesTree.Nearest(path[index].InformerNode.transform.position.ToArray(), radius).ToList();
* foreach (var informer in neighbours) {
* if (
* path.Exists(
* arg => arg.InformerNode.transform.position == informer.InformerNode.transform.position))
++intersection;
* }
* if (intersection <= 3) {
* if (loopflag) {
* loopflag = false;
* var loopend = path[i];
* //Debug.Log("Loopend: " + loopend.InformerNode.transform.position);
* var loopescape = Extensions.LoopEscape( loopstart, loopend, NodesTree, radius);
* if (loopescape != null) {
* finalPath.AddRange(loopescape);
* }
* loopstart = null;
* } else {
* finalPath.Add(path[i].InformerNode);
* }
* }
* }
* }*/
if (debugInformation != null)
{
debugInformation.FinalPath = finalPath;
}
/*Debug.Log("Final Path:");
* foreach (var informer in finalPath) {
* Debug.Log(informer.transform.name + " " + informer.transform.position);
* }*/
return(finalPath);
}
public List <Node> JPS(Informer from, Informer to, bool debugFlag, out DebugInformationAlgorithm debugInformation)
{
if (from == null || to == null)
{
Debug.LogError("Can't run JPS+. Enter proper from and to parameters!");
debugInformation = null;
return(null);
}
if (!IsPrecomputed)
{
Debug.Log("Precomputing...");
PrecomputeMap();
Debug.Log("Done!");
}
var finish = NodesArray[(int)to.transform.position.x / 3, (int)to.transform.position.z / 3];
var linesFromFinish = new StraightLinesFromNode(finish);
var start = new Tree_Node(null, NodesArray[(int)from.transform.position.x / 3, (int)from.transform.position.z / 3]);
start.Currentnode.Distance = Extensions.Metrics(start, finish);
var current = start;
var path = new List <Tree_Node>();
var observed = new List <Tree_Node> {
current
};
if (debugFlag)
{
debugInformation = new DebugInformationAlgorithm
{
From = from,
To = to,
Observed = new List <Node>(),
FinalPath = new List <Informer>(),
LinesFromFinish = StraightLinesFromNode.ToList(linesFromFinish, NodesArray)
};
}
else
{
debugInformation = null;
}
while (current.Currentnode != finish)
{
if (!observed.Exists(arg => arg.Currentnode.Visited != NodeState.Processed))
{
Debug.Log("No path was found");
if (debugFlag)
{
debugInformation.Observed = Extensions.ToNodes(
observed.Where(arg => arg.Currentnode.Visited == NodeState.Processed).
OrderBy(arg => arg.Level).ToList());
}
return(null);
}
observed[0].Currentnode.Visited = NodeState.Processed;
//Go to finish if in Target JP
current.Currentnode = Extensions.IsTargetJP(current.Currentnode, linesFromFinish);
if (current.Currentnode.TargetJP && Extensions.Reachable(current.Currentnode, finish, NodesArray))
{
finish.DestinationFromPrevious = Extensions.DestinationInverse(current.Currentnode.DestinationToFinish);
path.Add(current);
current = new Tree_Node(current, finish);
path.Add(current);
break;
}
//Find next nodes
//Neighbours
var neighbours = Extensions.Neighbours(current, NodesArray, finish);
//Target JP
var lines = new StraightLinesFromNode(current.Currentnode, Extensions.GetDestinationsFromNeighbours(neighbours));
var minMetrics = current.Currentnode.Distance;
var tempList = new List <Node>();
if (lines.Lines != null)
{
foreach (var lineFromFinish in linesFromFinish.Lines)
{
foreach (var line in lines.Lines)
{
var coordinates = StraightLine.Crossing(line, lineFromFinish);
if (coordinates != null &&
Extensions.Reachable(current.Currentnode, NodesArray[coordinates.X, coordinates.Y], NodesArray))
{
if (coordinates != null && Extensions.Reachable(NodesArray[coordinates.X, coordinates.Y], finish, NodesArray) &&
Extensions.Reachable(current.Currentnode, NodesArray[coordinates.X, coordinates.Y], NodesArray))
{
var tempNode = new Node(NodesArray[coordinates.X, coordinates.Y]);
tempNode.Distance = Extensions.Metrics(new Tree_Node(current, tempNode), finish);
tempNode.TargetJP = true;
tempNode.DestinationToFinish = Extensions.DestinationInverse(lineFromFinish.Destination);
tempNode.Visited = NodeState.Discovered;
if (tempNode.Distance < minMetrics)
{
minMetrics = tempNode.Distance;
tempList.Clear();
tempList.Add(tempNode);
}
else if (Math.Abs(tempNode.Distance - minMetrics) < 0.00000000001)
{
tempNode.Distance = Extensions.Metrics(new Tree_Node(current, tempNode), finish);
tempList.Add(tempNode);
}
}
}
}
}
}
Tree_Node tempTargetJP = null;
if (tempList.Count != 0)
{
tempTargetJP = new Tree_Node(current, tempList[0]);
if (tempList.Count > 1)
{
var min = tempTargetJP.DistanceFromParent;
foreach (var node in tempList)
{
var tempMetrics = current.Currentnode.InformerNode.MetricsAStar(node.InformerNode);
if (tempMetrics < min)
{
tempTargetJP = new Tree_Node(current, node);
min = tempMetrics;
}
}
}
if (debugFlag)
{
debugInformation.CrossPoints.Add(tempTargetJP.Currentnode);
}
if (!observed.Exists(arg => arg.Currentnode.Position == tempTargetJP.Currentnode.Position))
{
observed.Add(tempTargetJP);
}
else
{
var index =
observed.FindIndex(arg => arg.Currentnode.Position == tempTargetJP.Currentnode.Position);
if (observed[index].Currentnode.Visited == NodeState.Discovered)
{
observed[index].Currentnode.Distance = tempTargetJP.Currentnode.Distance;
}
}
}
if (neighbours.Count != 0)
{
foreach (var neighbour in neighbours)
{
if (!observed.Exists(arg => arg.Currentnode.Position == neighbour.Currentnode.Position))
{
if (Extensions.SelectJPFromNeighbours(current, neighbour))
{
observed.Add(neighbour);
}
}
else
{
var index =
observed.FindIndex(arg => arg.Currentnode.Position == neighbour.Currentnode.Position);
if (observed[index].Currentnode.Visited == NodeState.Discovered)
{
observed[index].Currentnode.Distance = neighbour.Currentnode.Distance;
}
}
}
}
observed = observed.OrderBy(arg => arg.Currentnode.Visited).
ThenBy(arg => arg.Currentnode.Distance).ToList();
path.Add(current);
if (!observed[0].Currentnode.TargetJP && tempTargetJP != null)
{
current = observed[0].DistanceFromParent < tempTargetJP.DistanceFromParent ? observed[0] : tempTargetJP;
}
else
{
current = observed[0];
}
}
Debug.Log("Path: " + path.Count);
if (path.Count > 1)
{
var finalPath = new List <Node>();
while (current != start)
{
var middlePoints = StraightLine.FindMiddlePoints(current.Parent, current.Currentnode);
if (current.Parent != start.Currentnode)
{
middlePoints.RemoveAt(0);
}
finalPath.InsertRange(0, Extensions.ToNodes(middlePoints, NodesArray));
current = path.Find(arg => arg.Currentnode.Position == current.Parent.Position &&
arg.Level == current.Level - 1);
}
if (debugFlag)
{
debugInformation.Observed = Extensions.ToNodes(
observed.Where(arg => arg.Currentnode.Visited == NodeState.Processed).
OrderBy(arg => arg.Level).ToList());
debugInformation.FinalPath = Extensions.ToInformers(finalPath);
}
Debug.Log("Final path: " + finalPath.Count);
return(finalPath);
}
else
{
return(null);
}
}
