本文整理汇总了C++中GridMap::GetCellFromPoint方法的典型用法代码示例。如果您正苦于以下问题：C++ GridMap::GetCellFromPoint方法的具体用法？C++ GridMap::GetCellFromPoint怎么用？C++ GridMap::GetCellFromPoint使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类GridMap的用法示例。
在下文中一共展示了GridMap::GetCellFromPoint方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: PlanUsingReedSheppWithObstacleDetection
double PlannerH::PlanUsingReedSheppWithObstacleDetection(const WayPoint& start, const WayPoint& goal,GridMap& map, vector<WayPoint>& genSmoothedPath,
const double pathDensity , const double smoothFactor )
{
RSPlanner rs_planner(smoothFactor);
int numero = 0;
double t=0, u=0 , v=0;
rs_planner.PATHDENSITY = pathDensity;
genSmoothedPath.clear();
genSmoothedPath.clear();
double length = rs_planner.min_length_rs(start.pos.x, start.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(start.pos.a), goal.pos.x, goal.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(goal.pos.a), numero, t , u , v);
rs_planner.constRS(numero, t, u , v, start.pos.x, start.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(start.pos.a), rs_planner.PATHDENSITY, genSmoothedPath);
if(genSmoothedPath.size() == 0)
return length;
CELL_Info* pCellRet = 0;
WayPoint p = genSmoothedPath.at(0);
int nChanges = 0;
double nMinChangeDistance = length;
double d = 0;
for(unsigned int i=0; i<genSmoothedPath.size(); i++)
{
if(p.bDir != genSmoothedPath.at(i).bDir)
{
if(d < nMinChangeDistance)
nMinChangeDistance = d;
d = 0;
nChanges++;
}
d+= distance2points(p.pos, genSmoothedPath.at(i).pos);
p = genSmoothedPath.at(i);
// if(map.)
// pCellRet = map.GetCellFromPointInnerMap(p.p);
// else
pCellRet = map.GetCellFromPoint(POINT2D(p.pos.x, p.pos.y));
if(pCellRet)
{
if(pCellRet->nMovingPoints > 0|| pCellRet->nStaticPoints > 0 || pCellRet->heuristic == map.m_MaxHeuristics)
{
cout << "\n Obstacle Detected \n";
genSmoothedPath.clear();
return -1;
}
}
else
{
cout << "\n Outside the Main Grid \n";
genSmoothedPath.clear();
return -1;
}
}
if(nChanges > 3 || nMinChangeDistance < 3.2)
{
cout << "\n Too much gear changes \n";
genSmoothedPath.clear();
return -1;
}
// pthread_mutex_lock(&planning_mutex);
// m_CurrentPath.assign(genSmoothedPath.begin(), genSmoothedPath.end());
// m_CurrentSmoothPath.clear();
// //m_TotalPath.assign(m_CurrentPath.begin(), m_CurrentPath.end());
// m_CurrentSmoothPath.assign(m_CurrentPath.begin(), m_CurrentPath.end());
// //m_TotalSmoothPath.assign(m_CurrentSmoothPath.begin(), m_CurrentSmoothPath.end());
// pthread_mutex_unlock(&planning_mutex);
return length;
}