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