/// <summary>
///
/// </summary>
/// <param name="searchParameters"></param>
public PathFinder(SearchParameters searchParameters)
{
_searchParameters = searchParameters;
InitializeNodes(searchParameters.Map);
_startNode = _nodes[(int)searchParameters.StartLocation.x, (int)searchParameters.StartLocation.y];
_startNode.State = Node.NodeState.Open;
_endNode = _nodes[(int)searchParameters.EndLocation.x, (int)searchParameters.EndLocation.y];
}
public Grid(Terrain terrain, float sample, float heightCost)
{
_terrain = terrain;
_worldPosition = terrain.GetPosition();
var width = (int)terrain.terrainData.size.x;
var height = (int)terrain.terrainData.size.z;
SampledWidth = (int)Mathf.Ceil(width / sample);
SampledHeight = (int)Mathf.Ceil(height / sample);
_sample = sample;
_heightCost = heightCost;
InternalGrid = new Node[SampledWidth, SampledHeight];
InternalGridWithUnwalkable = new Node[SampledWidth, SampledHeight];
//Build the Grid [width,height] --> O(n^2)
InitGrid();
InitGridWithUnWalkable();
//Initialize the edges/neighboots --> O(n^2)
InitEdges();
InitEdgesWithUnWalkable();
}
/// <summary>
///
/// </summary>
/// <param name="currentNode"></param>
/// <returns></returns>
private bool Search(Node currentNode)
{
currentNode.State = Node.NodeState.Closed;
List<Node> nextNodes = GetAdjacentWalkableNodes(currentNode);
nextNodes.Sort((node1, node2) => node1.F.CompareTo(node2.F));
foreach (var nextNode in nextNodes)
{
if (nextNode.Location == _endNode.Location)
{
return true;
}
if (Search(nextNode))
{
return true;
}
}
return false;
}
/// <summary>
///
/// </summary>
/// <param name="map"></param>
private void InitializeNodes(bool[,] map)
{
_width = map.GetLength(0);
_height = map.GetLength(1);
_nodes = new Node[_width, _height];
for (int y = 0; y < _height; y++)
{
for (int x = 0; x < _width; x++)
{
_nodes[x, y] = new Node(x, y, map[x, y], _searchParameters.EndLocation);
}
}
}
/// <summary>
///
/// </summary>
/// <param name="fromNode"></param>
/// <returns></returns>
private List<Node> GetAdjacentWalkableNodes(Node fromNode)
{
List<Node> walkableNodes = new List<Node>();
IEnumerable<Vector2> nextLocations = GetAdjacentLocations(fromNode.Location);
foreach (var location in nextLocations)
{
int x = (int)location.x;
int y = (int)location.y;
if (x < 0 || x >= _width || y < 0 || y >= _height)
{
continue;
}
Node node = _nodes[x, y];
if (!node.IsWalkable)
{
continue;
}
if (node.State == Node.NodeState.Closed)
{
continue;
}
if (node.State == Node.NodeState.Open)
{
float traversalCost = Node.GetTraversalCost(node.Location, node.ParentNode.Location);
float gTemp = fromNode.G + traversalCost;
if (gTemp < node.G)
{
node.ParentNode = fromNode;
walkableNodes.Add(node);
}
}
else
{
node.ParentNode = fromNode;
node.State = Node.NodeState.Open;
walkableNodes.Add(node);
}
}
return walkableNodes;
}
private void InitGridWithUnWalkable()
{
LayerMask mask = 1 << 2; //it's ignore raycast
for (var i = 0; i < SampledWidth; i++)
{
for (var j = 0; j < SampledHeight; j++)
{
var x = _sample * 0.5f * (2 * i + 1); //I calculate the center of each grid i * sample + sample / 2
var z = _sample * 0.5f * (2 * j + 1);
float attenuation = 0;
var pos = new Vector3(x, 0, z) + _worldPosition;
var terrainheight = _terrain.SampleHeight(pos);
pos.y = terrainheight;
RaycastHit hit;
var intersect = Physics.Raycast(pos + new Vector3(0, 10, 0), Vector3.down, out hit, Mathf.Infinity, ~mask);
if (intersect)
{
var colliderSize = hit.collider.bounds.size;
var colliderHeight = colliderSize.y;
attenuation = colliderHeight;
}
var cost = intersect ? int.MaxValue : 1;
attenuation = (terrainheight + attenuation);
var node = new Node(cost, attenuation, new Location(i, j), pos, isWalkable: !intersect);
InternalGridWithUnwalkable[i, j] = node;
}
}
}
private IEnumerable<Node> FindNeighboors(Node node, int i, int j)
{
var neighboors = new List<Node>(4);
foreach (var point in _directions)
{
if (0 > i + point.X || i + point.X >= SampledWidth || 0 > j + point.Y || j + point.Y >= SampledHeight)
continue;
var neighNode = InternalGrid[i + point.X, j + point.Y];
if (neighNode.IsWalkable)
neighboors.Add(neighNode);
}
return neighboors;
}
/// <summary>
/// Heuristics on the specified Node based on the Manhattan distance
/// </summary>
/// <param name="a">The Node a.</param>
/// <param name="b">The goal Node b.</param>
/// <returns></returns>
private static float Heuristic(Node a, Node b)
{
var dx = Mathf.Abs(a.Position.x - b.Position.x);
var dz = Mathf.Abs(a.Position.z - b.Position.z);
var dy = Mathf.Abs(a.Position.y - b.Position.y);
return 1.0f * (dx + dy + dz);
}
public List<Vector3> ForceSearch(Node startN, Node endN, Node targetNode)
{
throw new NotImplementedException();
}