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