public List <PathFindNode> GetRoad(int numCheckpoints, System.Random rnd)
{
List <PathFindNode> path = new List <PathFindNode>();
path.Clear();
PathFindNode start = GetNewRoadGoal(null, path, rnd, false);
PathFindNode target = null;
for (int i = 0; i < numCheckpoints; i++)
{
if (i == numCheckpoints - 1)
{
target = path[0];
}
else
{
target = GetNewRoadGoal(start, path, rnd, i % 2 == 0);
}
LinkedList <PathFindNode> p = PathFind(start, target);
if (p == null)
{
if (i == 0)
{
start = target;
}
i--;
continue;
}
start = target;
AddRoadSegment(path, p);
}
return(path);
}
public float Distance(PathFindNode other)
{
float dx = x - other.x;
float dy = y - other.y;
return(Mathf.Sqrt(dx * dx + dy * dy));
}
private void PathFindTryPath(PathFindNode from, PathFindNode to, PathFindNode target, LinkedList <PathFindNode> queue)
{
if (to == null || to.visited)
{
return;
}
float distance = from.distance + ((from.height - to.height) * heightPenalty) * ((from.height - to.height) * heightPenalty) + from.Distance(to);
if (distance + 0.1f < to.distance)
{
to.distance = distance;
to.prevNode = from;
to.estimation = to.distance + to.Distance(target);
if (to.queueNode != null)
{
queue.Remove(to.queueNode);
}
if (queue.Count == 0)
{
queue.AddFirst(to);
}
var pos = queue.First;
while (pos.Next != null && to.estimation > pos.Next.Value.estimation)
{
pos = pos.Next;
}
to.queueNode = queue.AddAfter(pos, to);
}
}
public void Remove(PathFindNode node)
{
if (node.QueueIndex == numNodes)
{
nodes[numNodes--] = null;
return;
}
var formerLastNode = nodes[numNodes];
Swap(node, formerLastNode);
nodes[numNodes--] = null;
var parentIndex = formerLastNode.QueueIndex / 2;
var parentNode = nodes[parentIndex];
if (parentIndex > 0 && CompareTo(formerLastNode, parentNode) >= 0)
{
SortUp(formerLastNode);
}
else
{
SortDown(formerLastNode);
}
}
public int SqrDistance(PathFindNode other)
{
int dx = x - other.x;
int dy = y - other.y;
return(dx * dx + dy * dy);
}
public void Reset()
{
distance = 99999f;
prevNode = null;
queueNode = null;
visited = false;
estimation = float.PositiveInfinity;
}
public LinkedList <PathFindNode> PathFind(PathFindNode start, PathFindNode target)
{
int width = nodes.GetLength(0);
int height = nodes.GetLength(1);
if (!Utils.InMargin(start.x, start.y, width, height, 1) || !Utils.InMargin(target.x, target.y, width, height, 1) || target == null || start == null)
{
return(null);
}
for (int i = 0; i < width; i++)
{
for (int j = 0; j < height; j++)
{
if (nodes[i, j] != null)
{
nodes[i, j].Reset();
}
}
}
int jumpDistance = 1;
LinkedList <PathFindNode> queue = new LinkedList <PathFindNode>();
start.distance = 0;
queue.AddFirst(start);
while (queue.Count > 0)
{
PathFindNode current = queue.First.Value;
queue.RemoveFirst();
current.queueNode = null;
current.visited = true;
if (current == target)
{
LinkedList <PathFindNode> path = new LinkedList <PathFindNode>();
while (target != null)
{
path.AddFirst(target);
target = target.prevNode;
}
return(path);
}
PathFindTryPath(current, nodes[current.x + jumpDistance, current.y], target, queue);
PathFindTryPath(current, nodes[current.x - jumpDistance, current.y], target, queue);
PathFindTryPath(current, nodes[current.x, current.y + jumpDistance], target, queue);
PathFindTryPath(current, nodes[current.x, current.y - jumpDistance], target, queue);
PathFindTryPath(current, nodes[current.x + jumpDistance, current.y + jumpDistance], target, queue);
PathFindTryPath(current, nodes[current.x + jumpDistance, current.y - jumpDistance], target, queue);
PathFindTryPath(current, nodes[current.x - jumpDistance, current.y + jumpDistance], target, queue);
PathFindTryPath(current, nodes[current.x - jumpDistance, current.y - jumpDistance], target, queue);
if (queue.Count > nodes.Length / 8)
{
return(null);
}
}
return(null);
}
private void Swap(PathFindNode node1, PathFindNode node2)
{
nodes[node1.QueueIndex] = node2;
nodes[node2.QueueIndex] = node1;
var temp = node1.QueueIndex;
node1.QueueIndex = node2.QueueIndex;
node2.QueueIndex = temp;
}
private static int GetDistance(PathFindNode nodeA, PathFindNode nodeB)
{
var dstX = Math.Abs(nodeA.X - nodeB.X);
var dstZ = Math.Abs(nodeA.Z - nodeB.Z);
var dstY = Math.Abs(nodeA.Position.y - nodeB.Position.y);
if (dstX > dstZ)
{
return(14 * dstZ + 10 * (dstX - dstZ) + (int)(10 * dstY));
}
return(14 * dstX + 10 * (dstZ - dstX) + (int)(10 * dstY));
}
private static List <Vector3> RetracePath(PathFindNode startNode, PathFindNode endNode)
{
var path = new List <Vector3>();
while (endNode != startNode)
{
path.Add(endNode.Position);
endNode = endNode.Parent;
}
path.Reverse();
//path.RemoveAt(0);
return(path);
}
private void SortDown(PathFindNode node)
{
var finalQueueIndex = node.QueueIndex;
while (true)
{
var newParent = node;
var childLeftIndex = 2 * finalQueueIndex;
if (childLeftIndex > numNodes)
{
node.QueueIndex = finalQueueIndex;
nodes[finalQueueIndex] = node;
break;
}
var childLeft = nodes[childLeftIndex];
if (CompareTo(childLeft, newParent) >= 0)
{
newParent = childLeft;
}
var childRightIndex = childLeftIndex + 1;
if (childRightIndex <= numNodes)
{
var childRight = nodes[childRightIndex];
if (CompareTo(childRight, newParent) >= 0)
{
newParent = childRight;
}
}
if (newParent != node)
{
nodes[finalQueueIndex] = newParent;
var temp = newParent.QueueIndex;
newParent.QueueIndex = finalQueueIndex;
finalQueueIndex = temp;
}
else
{
node.QueueIndex = finalQueueIndex;
nodes[finalQueueIndex] = node;
break;
}
}
}
private static PathFindNode FindPathNodeOrCreate(int x, int z, float y)
{
var node = Grid[x, z];
if (node != null)
{
return(node);
}
var halfGrid = Size / 2f;
var groundPos = new Vector3(x - halfGrid, y, z - halfGrid);
//groundPos.y = TerrainMeta.HeightMap.GetHeight(groundPos);
FindRawGroundPosition(groundPos, out groundPos);
Grid[x, z] = node = new PathFindNode(groundPos);
return(node);
}
private PathFindNode GetNeighbourIfExistOrClose(PathFindNode currentEndNode, Vector3 currentPosition, Vector3 targetPosition)
{
float distanceBetweenCurrentAndNode = MathUtils.GetDistance2D(currentPosition, currentEndNode.transform.position);
if (currentEndNode.neighbours.Length > 0)
{
PathFindNode pathfindNode = currentEndNode.neighbours [0];
float distanceBetweenCurrentAndNeighbour = MathUtils.GetDistance2D(currentPosition, pathfindNode.transform.position);
if (distanceBetweenCurrentAndNeighbour < (distanceBetweenCurrentAndNode * extraDistancePenalityForNonNeighbour))
{
return(pathfindNode);
}
}
return(null);
}
private PathFindNode FindNodeClosestTo(List <PathFindNode> nodesToUse, Vector3 goalPosition)
{
float closestDistance = float.MaxValue;
PathFindNode closestNode = null;
foreach (PathFindNode pathFindNode in nodesToUse)
{
float distance = MathUtils.GetDistance2D(goalPosition, pathFindNode.transform.position);
if (distance < closestDistance)
{
closestDistance = distance;
closestNode = pathFindNode;
}
}
return(closestNode);
}
private void SortUp(PathFindNode node)
{
var parent = node.QueueIndex / 2;
while (parent > 0)
{
var parentNode = nodes[parent];
if (CompareTo(parentNode, node) >= 0)
{
break;
}
Swap(node, parentNode);
parent = node.QueueIndex / 2;
}
}
private PathFindNode FindNeighbourClosestToNode(PathFindNode currentEndNode, Vector3 currentPosition, Vector3 targetPosition)
{
float closestDistance = float.MaxValue;
PathFindNode closestNode = null;
float distanceBetweenGoalAndEndNode = MathUtils.GetDistance2D(currentPosition, currentEndNode.transform.position);
foreach (PathFindNode pathfindNode in currentEndNode.neighbours)
{
float distanceBetweenCurrentAndNeighbour = MathUtils.GetDistance2D(currentPosition, pathfindNode.transform.position);
if (distanceBetweenCurrentAndNeighbour < closestDistance && distanceBetweenCurrentAndNeighbour < distanceBetweenGoalAndEndNode)
{
closestDistance = distanceBetweenCurrentAndNeighbour;
closestNode = pathfindNode;
}
}
return(closestNode);
}
private static int CompareTo(PathFindNode node, PathFindNode other)
{
if (node.F == other.F)
{
if (node.H == other.H)
{
return(0);
}
if (node.H > other.H)
{
return(-1);
}
return(1);
}
if (node.F > other.F)
{
return(-1);
}
return(1);
}
private PathFindNode GetNewRoadGoal(PathFindNode start, List <PathFindNode> road, System.Random rnd, bool goalClose = false)
{
PathFindNode node = null;
if (start == null)
{
while (node == null)
{
node = nodes[rnd.Next(1, nodes.GetLength(0) - 1), rnd.Next(1, nodes.GetLength(0) - 1)];
}
return(node);
}
bool prev10 = true;
do
{
node = nodes[rnd.Next(1, nodes.GetLength(0) - 1), rnd.Next(1, nodes.GetLength(0) - 1)];
if (node == null)
{
continue;
}
if (goalClose && node.SqrDistance(start) > nodes.Length / 16)
{
continue;
}
prev10 = false;
for (int pn = Mathf.Max(0, road.Count - 10); pn < road.Count; pn++)
{
if (node.SqrDistance(road[pn]) <= 9)
{
prev10 = true;
break;
}
}
}while (prev10);
return(node);
}
public List <PathFindNode> FindBestPathToTargetFrom(List <PathFindNode> allNodes, Vector3 currentPosition, Vector3 target)
{
this.openNodes = allNodes;
this.closedNodes = new List <PathFindNode> ();
PathFindNode nodeClosestToTarget = FindNodeClosestTo(openNodes, target);
PathFindNode currentStartNode = nodeClosestToTarget;
closedNodes.Add(nodeClosestToTarget);
while (currentStartNode != null)
{
currentStartNode = GetNeighbourIfExistOrClose(currentStartNode, currentPosition, target);
if (currentStartNode)
{
closedNodes.Add(currentStartNode);
}
}
closedNodes.Reverse();
return(closedNodes);
}
public PathFindNode(TileSpawnData t, PathFindNode p)
{
tile = t;
parent = p;
}
public bool Contains(PathFindNode node)
{
return(nodes[node.QueueIndex] == node);
}
public void Update(PathFindNode node)
{
SortUp(node);
}
public List <Vector3> FindPath(Vector3 sourcePos, Vector3 targetPos)
{
//Interface.Oxide.LogInfo("Queue: {0} Grid: {1}", OpenList.MaxSize, Grid.Length);
var closedList = new HashSet <PathFindNode>();
var targetNode = new PathFindNode(targetPos);
if (targetNode.X < 0 || targetNode.X >= Size || targetNode.Z < 0 || targetNode.Z >= Size)
{
return(null);
}
Grid[targetNode.X, targetNode.Z] = targetNode;
var startNode = new PathFindNode(sourcePos);
if (startNode.X < 0 || startNode.X >= Size || startNode.Z < 0 || startNode.Z >= Size)
{
return(null);
}
Grid[startNode.X, startNode.Z] = startNode;
OpenList.Enqueue(startNode);
//PathFindNode closestNode = null;
while (OpenList.Count > 0)
{
var currentNode = OpenList.Dequeue();
if (currentNode == targetNode)
{
Clear();
return(RetracePath(startNode, targetNode));
}
closedList.Add(currentNode);
for (var i = 0; i < 8; i++)
{
var dirX = Direction[i, 0];
var dirZ = Direction[i, 1];
var x = currentNode.X + dirX;
var z = currentNode.Z + dirZ;
if (x < 0 || x >= Size || z < 0 || z >= Size)
{
continue;
}
var neighbour = FindPathNodeOrCreate(x, z, currentNode.Position.y);
//Interface.Oxide.LogInfo("Checking neighbour: {0} {1} {2} {3} {4}", x, z, neighbour.Position, closedList.Contains(neighbour), neighbour.Walkable);
if (!neighbour.Walkable)
{
continue;
}
var newGScore = currentNode.G + GetDistance(currentNode, neighbour);
if (newGScore >= neighbour.G && closedList.Contains(neighbour))
{
continue;
}
if (newGScore < neighbour.G || !OpenList.Contains(neighbour))
{
//foreach (var player in BasePlayer.activePlayerList)
// player.SendConsoleCommand("ddraw.sphere", 30f, Color.black, neighbour.Position, .25f);
neighbour.G = newGScore;
neighbour.H = GetDistance(neighbour, targetNode);
neighbour.F = newGScore + neighbour.H;
neighbour.Parent = currentNode;
if (!OpenList.Contains(neighbour))
{
OpenList.Enqueue(neighbour);
}
else
{
OpenList.Update(neighbour);
}
//if (closestNode == null || newGScore < closestNode.G)
// closestNode = neighbour;
}
}
if (closedList.Count > MaxDepth)
{
//Interface.Oxide.LogWarning("[PathFinding] Hit MaxDepth!");
break;
}
}
Clear();
//if (closestNode != null)
// return RetracePath(startNode, closestNode);
return(null);
}
public void Enqueue(PathFindNode node)
{
nodes[++numNodes] = node;
node.QueueIndex = numNodes;
SortUp(node);
}
