private Vector3D[] FindPath()
{
Vector3D[] waypoints = new Vector3D[0];
if (_closestNodeToStart == null || _closestNodeToEnd == null || _closestNodeToStart == _closestNodeToEnd)
{
return(waypoints);
}
bool pathSuccess = false;
_closestNodeToStart.Parent = _closestNodeToStart;
Heap <PathNode> openSet = new Heap <PathNode>(_steps * _steps);
HashSet <PathNode> closedSet = new HashSet <PathNode>();
openSet.Add(_closestNodeToStart);
while (openSet.Count > 0)
{
PathNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == _closestNodeToEnd)
{
pathSuccess = true;
break;
}
foreach (var neighbour in GetNeighbours(currentNode))
{
if (closedSet.Contains(neighbour))
{
continue;
}
int newMoveCostToNeighbour = currentNode.CostG + GetDistance(currentNode, neighbour);
if (newMoveCostToNeighbour < neighbour.CostG || !openSet.Contains(neighbour))
{
neighbour.CostG = newMoveCostToNeighbour;
neighbour.CostH = GetDistance(neighbour, _closestNodeToEnd);
neighbour.Parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(_closestNodeToStart, _closestNodeToEnd);
}
return(waypoints);
}
private void CalculateAreaAndPath()
{
//Reset Existing Values
_closestNodeToStart = null;
_closestNodeToEnd = null;
_endPath = false;
_goodNodes.Clear();
_badNodes.Clear();
_pathWaypoints.Clear();
_shorelineNodes.Clear();
if (!EnablePathGeneration)
{
_endPath = true;
return;
}
//Create The Grid Of Nodes
var up = _behavior.AutoPilot.UpDirectionFromPlanet;
var forward = Vector3D.CalculatePerpendicularVector(up);
var myPos = _behavior.RemoteControl.GetPosition();
var myMatrix = MatrixD.CreateWorld(myPos, forward, up);
double distanceOffsetHalf = (_worldStepDistance * _steps) / 2;
var startOffset = new Vector3D(-distanceOffsetHalf, 0, -distanceOffsetHalf);
var startGridCoords = Vector3D.Transform(startOffset, myMatrix);
_targetCoords = _behavior.AutoPilot.GetPendingWaypoint();
for (int x = 0; x <= _steps; x++)
{
for (int y = 0; y <= _steps; y++)
{
var gridWorldCellOffset = startOffset;
gridWorldCellOffset.X += (x * _worldStepDistance);
gridWorldCellOffset.Z += (y * _worldStepDistance);
var gridWorldCoords = Vector3D.Transform(gridWorldCellOffset, myMatrix);
double distAboveWater = 0;
var surfaceCoords = WaterHelper.GetClosestSurface(gridWorldCoords, _behavior.AutoPilot.CurrentPlanet, _behavior.AutoPilot.CurrentWater, ref distAboveWater);
if (x == 0 & y == 0)
{
_gridStartCoords = surfaceCoords;
}
if (x == _steps - 1 & y == _steps - 1)
{
_gridEndCoords = surfaceCoords;
}
var node = new PathNode(surfaceCoords, x, y);
if (distAboveWater > -_minimumDepth)
{
_badNodes.Add(node);
continue;
}
_goodNodes.Add(node);
node.DistanceFromStart = Vector3D.Distance(myPos, surfaceCoords);
node.DistanceFromEnd = Vector3D.Distance(surfaceCoords, _targetCoords);
if (_closestNodeToStart == null || node.DistanceFromStart < _closestNodeToStart.DistanceFromStart)
{
_closestNodeToStart = node;
}
if (_closestNodeToEnd == null || node.DistanceFromEnd < _closestNodeToEnd.DistanceFromEnd)
{
_closestNodeToEnd = node;
}
}
}
for (int i = _goodNodes.Count - 1; i >= 0; i--)
{
if (GetNeighbours(_goodNodes[i], true).Count > 0)
{
_shorelineNodes.Add(_goodNodes[i]);
_goodNodes.RemoveAt(i);
}
}
if (_shorelineNodes.Contains(_closestNodeToStart))
{
_closestNodeToStart = null;
foreach (var node in _goodNodes)
{
if (_closestNodeToStart == null || node.DistanceFromStart < _closestNodeToStart.DistanceFromStart)
{
_closestNodeToStart = node;
}
}
}
if (_shorelineNodes.Contains(_closestNodeToEnd))
{
_closestNodeToEnd = null;
foreach (var node in _goodNodes)
{
if (_closestNodeToEnd == null || node.DistanceFromEnd < _closestNodeToEnd.DistanceFromEnd)
{
_closestNodeToEnd = node;
}
}
}
//Logger.MsgDebug("Total Nodes Calculated: " + _goodNodes.Count, DebugTypeEnum.AutoPilot);
//Calculate the Path
_pathWaypoints = new List <Vector3D>(FindPath());
}
