public static Point Crossing(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4)
{
var isCrossing = false;
var line1 = new StraightLine(new Point(x1, y1), new Point(x2, y2));
var line2 = new StraightLine(new Point(x3, y3), new Point(x4, y4));
var crossPoint = new Point();
var a1 = y1 - y2;
var b1 = x2 - x1;
var c1 = x1 * y2 - x2 * y1;
var a2 = y3 - y4;
var b2 = x4 - x3;
var c2 = x3 * y4 - x4 * y3;
if (a1 * b2 - a2 * b1 != 0)
{
crossPoint.X = -(c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1);
crossPoint.Y = -(a1 * c2 - a2 * c1) / (a1 * b2 - a2 * b1);
if (crossPoint.Belongs(line1) && crossPoint.Belongs(line2))
{
isCrossing = true;
}
}
else
{
return(line1.Belongs(line2) || line2.Belongs(line1) ? new Point(-100, -100) : null);
}
return(isCrossing ? crossPoint : null);
}
public static bool OnOneLine(Node sNode, Node fNode, Node[,] array)
{
var destination = FindDestination(sNode, fNode);
var line = new StraightLine(sNode.X(), sNode.Y(), destination);
var obstacleIndex = -1;
for (var i = 0; i < line.Points.Count; ++i)
{
if (array[line.Points[i].X, line.Points[i].Y].InformerNode.IsObstacle)
{
obstacleIndex = i;
break;
}
}
if (obstacleIndex != -1)
{
line.Points.RemoveRange(obstacleIndex, line.Points.Count - obstacleIndex - 1);
}
return(line.Points.Exists(arg => arg.X == fNode.X() && arg.Y == fNode.Y()));
}
public static Point Crossing(StraightLine line1, StraightLine line2)
{
var isCrossing = false;
var crossPoint = new Point();
foreach (var pointInLine1 in line1.Points)
{
if (pointInLine1.Belongs(line2))
{
isCrossing = true;
crossPoint = pointInLine1;
break;
}
}
return(isCrossing ? crossPoint : null);
/*return Crossing(line1.Start.X, line1.Start.Y, line1.Finish.X, line1.Finish.Y,
* line2.Start.X, line2.Start.Y, line2.Finish.X, line2.Finish.Y);*/
}
public bool Belongs(StraightLine line)
{
return(line.Points.Exists(arg => arg.X == X && arg.Y == Y));
}
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);
}
}
public bool Belongs(StraightLine line)
{
bool belongs = line != null && Start.Belongs(line) && Finish.Belongs(line);
return(belongs);
}
