C++ AStar类代码示例

AStar* AStar::create(int ** map,int width,int height,bool useApproximateDest){
    AStar*  instance = new AStar(map,width,height,useApproximateDest);
    if (instance->init()) {
        return instance;
    }
    return nullptr;
}

int main()
{
    out.open("log.txt");

    GameMap gmap(50,50);
    gmap.load_from_file("map.txt");

    NodePtr src(new Node(Coordinate(0,0)));
    NodePtr dest(new Node(Coordinate(4,3)));

    AStar astar;
    Path res;
    res = astar.get_shorten_path(gmap, src, dest);
    Path::iterator it = res.begin();
    for (; it!=res.end(); ++it)
    {
        cout << (*it)->get_coordinate() << " ";
        gmap.set_cell((*it)->get_coordinate().x,(*it)->get_coordinate().y,'>');
    }
    cout << endl;

    gmap.dump("map_res.txt");

    std::cout << "Hello world!" << std::endl;
    return 0;
}

int main()
{
	int * main_map;
	AStar star;
	main_map = generateMap(10,10);
	bool result=star.Init(main_map, 10, 10);
	star.AStarSearch(1, 99);
	return 0;
}

int main(){
    int **map = new int*[10 * 10];
    memset(map, 0, 10 * 10);

    AStar* astar = AStar::create(map,10,10);
    vector<ASNode> path = astar->findPath(0,1,9,9);
    astar->printPath(path);
    return 1;
}

int main()
{	
	sf::RenderWindow window(sf::VideoMode(Constants::screenWidth, Constants::screenHeight), "Puzzle15");
	
	Puzzle15 puzzle;

	srand(time(NULL));
	Configuration config;

	int solvable[16] ={ 2, 5, 3, 4, 1, 10, 6, 8, 9, 14, 7, 11, 13, 16, 15, 12 };
	config.setConfiguration(solvable);

	puzzle.changeConfiguration(config);

	std::vector<Configuration> result;

	AStar solver;
	solver.Run(config, result);

	//puzzle.changeConfiguration(config);

	while (window.isOpen())
	{
		sf::Event event;
		while (window.pollEvent(event))
		{
			if (event.type == sf::Event::Closed)
				window.close();
			else
			{
				if (event.type == sf::Event::KeyPressed)
				{
					if (event.key.code == sf::Keyboard::Space)
					{
						if (!result.empty())
						{
							puzzle.changeConfiguration(result.back());
							result.pop_back();
						}
					}
				}
			}
		}

		window.clear();
		puzzle.draw(&window);
		window.display();
	}
	
	return 0;

	/*AStar algorithm;
	algorithm.Run();
	system("pause");*/
}

int main(void)
{
	//Puzzle::PuzzleState g = { 1, 2, 3, 8, 0, 4, 7, 6, 5 };
	//Puzzle start(3);
	//Puzzle goal(g, 3);

	AStar aStar;
	AStar::PuzzleState start = { { 5, 2, 0, 4, 7, 6, 8, 3, 1 }, 0, 0 };
	//AStar::PuzzleState start = { { 2, 3, 4, 1, 8, 0, 7, 6, 5 }, 0, 0 };
	AStar::PuzzleState goal = { { 1, 2, 3, 8, 0, 4, 7, 6, 5 }, 0, 0 };
	aStar.solve(start, goal);

	return (0);
}

AStar* AStar::create()
{
	AStar *pRet = new AStar();
	if (pRet && pRet->init())
	{
		pRet->autorelease();
		pRet->retain();
		return pRet;
	}
	else
	{
		delete pRet;
		pRet = NULL;
		return NULL;
	}	
}

int main(int argc, char** argv)
{
	AStar search;
	std::vector<RHNode*> results;

	gamestate start1;
	initTest1(start1);

	RHNode startNode1(start1);
	search.search(startNode1, results);

	printf("\nSuccess!\n");
	printf("\nSolution found with: \n");
	printf("%i moves", results.size());

}

void GridLayer::testAstar()
{
    int startX;
    int startY;
    int endX;
    int endY;
    
    for (int r = 0; r < m_row; r++) {
        for (int c = 0; c < m_col; c++) {
            if(m_data[r][c] == FLAG_PATH)
            {
                m_data[r][c] = FLAG_EMPTY;
            }
            
            if (m_data[r][c] == FLAG_START)
            {
                startX = c;
                startY = r;
            }
            
            if (m_data[r][c] == FLAG_END)
            {
                endX = c;
                endY = r;
            }
        }
    }
    
    AStar* astar = new AStar(m_data, m_row, m_col);
    std::vector<Step*> moveList = astar->getPath(startX, startY, endX, endY);
    delete astar;
    
    // 删除起点终点
    moveList.pop_back();
    std::vector<Step*>::iterator iter = moveList.begin();
    moveList.erase(iter);
    
    // 赋值
    for (auto step : moveList)
    {
        m_data[step->gy][step->gx] = 1;
    }
    

    
    infoMap();
}

AStar::AStar(const AStar& other) : numRows(other.getRowCount()), numCols(other.getColCount())
{
	map = new std::vector<std::vector<AStarNode*>*>(numRows);
	for(int i=0;i<numRows;i++)
	{
		map->at(i) = new std::vector<AStarNode*>(numCols);
		for (int j = 0; j < numCols; j++)
		{
			*(map->at(i)->at(j)) = *(other.map->at(i)->at(j));  //Double check this!!!!
																
		}
	}
	openList = new std::vector<AStarNode>(*other.getOpenList());
	closedList = new std::vector<AStarNode>(*other.getClosedList());
	goalNode = new AStarNode(other.getGoalNode());
	
}

void Manager::calculateWayPoint(){
	vector<Waypoint> vecWaypoints;

	Graph gr = _map->GetGraphFromGrid();

	AStar a;
	Node* start = gr.GetNodeByPos(_slam->BestPrticle()->GetX(), _slam->BestPrticle()->GetY());
	Node* end = gr.GetNodeByPos(40, 47);
	vector<Node*> vec = a.GetShortestPath(start, end);
	for (int i = vec.size() - 1; i > 0; --i) {
		Waypoint wp(vec.operator [](i)->getXPos(),vec.operator [](i)->getYPos());
		cout << "AStar path: (" << wp.getX() << "," << wp.getY() << ")" << endl;
		vecWaypoints.push_back(wp);
	}
	cout << "Finished AStar path" << endl;

	this->_robot->SetWaypointVector(vecWaypoints);
}

/** Search for a path between two nodes.
 * This function executes an A* search to find an (optimal) path
 * from node @p from to node @p to.
 * @param from node to search from
 * @param to goal node
 * @param estimate_func function to estimate the cost from any node to the goal.
 * Note that the estimate function must be admissible for optimal A* search. That
 * means that for no query may the calculated estimate be higher than the actual
 * cost.
 * @param cost_func function to calculate the cost from a node to another adjacent
 * node. Note that the cost function is directly related to the estimate function.
 * For example, the cost can be calculated in terms of distance between nodes, or in
 * time that it takes to travel from one node to the other. The estimate function must
 * match the cost function to be admissible.
 * @param use_constraints true to respect constraints imposed by the constraint
 * repository, false to ignore the repository searching as if there were no
 * constraints whatsoever.
 * @param compute_constraints if true re-compute constraints, otherwise use constraints
 * as-is, for example if they have been computed before to check for changes.
 * @return ordered vector of nodes which denote a path from @p from to @p to.
 * Note that the vector is empty if no path could be found (i.e. there is non
 * or it was prohibited when using constraints.
 */
fawkes::NavGraphPath
NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
		      navgraph::EstimateFunction estimate_func,
		      navgraph::CostFunction cost_func,
		      bool use_constraints, bool compute_constraints)
{
  if (! reachability_calced_)  calc_reachability();

  AStar astar;

  std::vector<AStarState *> a_star_solution;

  if (use_constraints) {
    constraint_repo_.lock();
    if (compute_constraints && constraint_repo_->has_constraints()) {
      constraint_repo_->compute();
    }

    NavGraphSearchState *initial_state =
      new NavGraphSearchState(from, to, this, estimate_func, cost_func,
			      *constraint_repo_);
    a_star_solution =  astar.solve(initial_state);
    constraint_repo_.unlock();
  } else {
    NavGraphSearchState *initial_state =
      new NavGraphSearchState(from, to, this, estimate_func, cost_func);
    a_star_solution =  astar.solve(initial_state);
  }

  std::vector<fawkes::NavGraphNode> path(a_star_solution.size());
  NavGraphSearchState *solstate;
  for (unsigned int i = 0; i < a_star_solution.size(); ++i ) {
    solstate = dynamic_cast<NavGraphSearchState *>(a_star_solution[i]);
    path[i] = solstate->node();
  }

  float cost =
    (! a_star_solution.empty())
      ? a_star_solution[a_star_solution.size() - 1]->total_estimated_cost
      : -1;

  return NavGraphPath(this, path, cost);
}

Widget::Widget(QWidget *parent) :
	QWidget(parent),
	ui(new Ui::Widget)
{
	ui->setupUi(this);

	int size = 3;
	QVector<QVector< GameObject *> > cells;

	cells = QVector<QVector <GameObject*> > (size);
	for (int i = 0; i < cells.size(); i++)
	{
		cells[i] = QVector <GameObject*> (size);
		for (int j = 0; j < cells[i].size(); j++)
		{
			cells[i][j] = nullptr;
		}
	}

	cells[0][0] = new GameObject();
	cells[0][1] = new TreeObject();
	cells[0][2] = new TreeObject();
	cells[1][0] = new GameObject();
	cells[1][1] = new GameObject();
	cells[1][2] = new GameObject();
	cells[2][0] = new GameObject();
	cells[2][1] = new GameObject();
	cells[2][2] = new GameObject();

	AStar *astar = new AStar();
	astar->addCells(cells);

	QList<QPoint *> path = astar->calcualtePath(QPoint(0, 0), QPoint(2, 2));

	QList<QPoint *>::iterator i;

	for (i = path.begin(); i != path.end(); ++i)
		qDebug() << (*i)->x() << (*i)->y() << '\n';
}

int main()
{
	char maps[1000][1000] =
	{
		{ 0, 1, 0, 0, 0, 1, 0, 0, 0, 0 },
		{ 0, 0, 0, 1, 0, 1, 0, 1, 0, 1 },
		{ 1, 1, 1, 1, 0, 1, 0, 1, 0, 1 },
		{ 0, 0, 0, 1, 0, 0, 0, 1, 0, 1 },
		{ 0, 1, 0, 1, 1, 1, 1, 1, 0, 1 },
		{ 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },
		{ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
		{ 0, 0, 0, 0, 1, 0, 0, 0, 1, 0 },
		{ 1, 1, 0, 0, 1, 0, 1, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 0, 1, 0, 1, 0 },
	};

	// 搜索参数
	AStar::Param param;
	param.width = 1000;
	param.height = 1000;
	param.allow_corner = false;
	param.start = AStar::Vec2(0, 0);
	param.end = AStar::Vec2(999, 999);
	param.is_canreach = [&](const AStar::Vec2 &pos)->bool
	{
		return maps[pos.y][pos.x] == 0;
	};

	// 执行搜索
	AStar as;
	Duration duration;
	auto path = as.search(param);

	std::cout << (path.empty() ? "路径未找到！" : "路径已找到！") << std::endl;
	std::cout << "本次寻路耗时" << duration.nano_seconds() << "纳秒" << std::endl;
	std::cout << '\n';
	system("pause");
	return 0;
}

int main(int argc, char *argv[])
{
	char maps[10][10] =
	{
		{ 0, 1, 0, 0, 0, 1, 0, 0, 0, 0 },
		{ 0, 0, 0, 1, 0, 1, 0, 1, 0, 1 },
		{ 1, 1, 1, 1, 0, 1, 0, 1, 0, 1 },
		{ 0, 0, 0, 1, 0, 0, 0, 1, 0, 1 },
		{ 0, 1, 0, 1, 1, 1, 1, 1, 0, 1 },
		{ 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },
		{ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
		{ 0, 0, 0, 0, 1, 0, 0, 0, 1, 0 },
		{ 1, 1, 0, 0, 1, 0, 1, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 0, 1, 0, 1, 0 },
	};

	// 搜索参数
	AStar::Param param;
	param.width = 10;
	param.height = 10;
	param.corner = false;
	param.start = AStar::Vec2(0, 0);
	param.end = AStar::Vec2(9, 9);
	param.can_reach = [&](const AStar::Vec2 &pos)->bool
	{
		return maps[pos.y][pos.x] == 0;
	};

	// 执行搜索
	AStar as;
	Duration duration;
	auto path = as.find(param);
	std::cout << (path.empty() ? "路径未找到！" : "路径已找到！") << std::endl;
	std::cout << "本次寻路耗时" << duration.nano_seconds() << "纳秒" << std::endl;
	std::cout << '\n';

	return 0;
}