|
马上注册,结交更多好友,享用更多功能^_^
您需要 登录 才可以下载或查看,没有账号?立即注册
x
本帖最后由 闪亮的马路 于 2022-3-24 09:41 编辑
结构体定义如下:
struct GridNode
{
int id;
Eigen::Vector3d coord;
Eigen::Vector3i dir; // direction of expanding
Eigen::Vector3i index;
double gScore, fScore;
GridNodePtr cameFrom;
std::multimap<double, GridNodePtr>::iterator nodeMapIt;
GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord){
id = 0;
index = _index;
coord = _coord;
dir = Eigen::Vector3i::Zero();
gScore = inf;
fScore = inf;
cameFrom = NULL;
}
GridNode(const GridNode& g);
GridNode(){};
~GridNode(){};
};
存储函数部分如下:
inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{
neighborPtrSets.clear();
edgeCostSets.clear();
Vector3i init_index(0,0,0);
Vector3d init_coord(0,0,0);
GridNodePtr neighborPtrSet = new GridNode(init_index, init_coord);
if (heuristic_type =="Manhattan")
{
//x-1
if(isFree(currentPtr->index[0]-1,currentPtr->index[1],currentPtr->index[2]) &&
!isOccupied(currentPtr->index[0]-1,currentPtr->index[1],currentPtr->index[2]))
{
neighborPtrSet->id = 0;
neighborPtrSet->index = currentPtr->index;
neighborPtrSet->index[0] = currentPtr->index[0]-1;
neighborPtrSet->coord = gridIndex2coord( neighborPtrSet->index);
neighborPtrSet->dir = currentPtr->dir;
neighborPtrSets.push_back(neighborPtrSet);
edgeCostSets.push_back(1);
}
//x+1
if(isFree(currentPtr->index[0]+1,currentPtr->index[1],currentPtr->index[2]) &&
!isOccupied(currentPtr->index[0]+1,currentPtr->index[1],currentPtr->index[2]))
{
neighborPtrSet->id = 0;
neighborPtrSet->index = currentPtr->index;
neighborPtrSet->index[0] = currentPtr->index[0]+1;
neighborPtrSet->coord = gridIndex2coord( neighborPtrSet->index);
neighborPtrSet->dir = currentPtr->dir;
neighborPtrSets.push_back(neighborPtrSet);
edgeCostSets.push_back(1);
}
仿真发现neighborPtrSets存储的两个结构体共用了一个地址,数据也一样,该怎么修改呢? |
|