private void AddDataForRecursion(Node currentNode, Obstacle foundObstacle)
{
foundObstacle.AddThrowedInThisPathSearch(currentNode.Point);
var data = new DataForRecursion();
data.currentNode = currentNode;
data.foundObstacle = foundObstacle;
var center = foundObstacle.GetCenter();
data.Heuristic = currentNode.TravelledDistance + Node.Distance(currentNode.Point, center) + MapConstants.NearestDistanceHeuristicValue * Node.Distance(center, _end);
//data.Heuristic = currentNode.Heuristic - currentNode.TravelledDistance;
//data.Heuristic = currentNode.Heuristic;
_dataForRecursion.Insert(data);
}
private void AddReachableVerticesFromObstacle(DataForRecursion data)
{
var currentNode = data.currentNode;
var end = _end;
var foundObstacle = data.foundObstacle;
for (var i = 0; i < foundObstacle.VerticesCount; i++)
{
var obstacleVertice = foundObstacle.GetVerticeAt(i);
//if (_openNodes.Find(obstacleVertice) != null)
//{
// continue; // а это хак?
//}
if (obstacleVertice.Equals(currentNode.Point))
{
continue; //TODO: и вот это хак
}
//var existingClosedNode = _closedNodes.FirstOrDefault(anotherNode => anotherNode.Point.Equals(obstacleVertice));
//if (existingClosedNode != null)
//{
// _debug.ClosedNode();
// continue; //TODO: и это хак, но это хороший хак, без него сломается
//}
Obstacle anotherIntersectedObstacle = FindFirstIntersection(currentNode.Point, obstacleVertice);
if (anotherIntersectedObstacle == null)
{
AddNode(currentNode.CreateChild(obstacleVertice, end));
continue;
}
if (anotherIntersectedObstacle == foundObstacle)
{
continue; //TODO: и это хак
}
if (anotherIntersectedObstacle.WasThrowedInThisPathSearch(currentNode.Point))
{
continue;
}
AddDataForRecursion(currentNode, anotherIntersectedObstacle);
}
}
