本文整理汇总了C++中WayPoints::computeDistanceBetweenPointsAlongRoad方法的典型用法代码示例。如果您正苦于以下问题:C++ WayPoints::computeDistanceBetweenPointsAlongRoad方法的具体用法?C++ WayPoints::computeDistanceBetweenPointsAlongRoad怎么用?C++ WayPoints::computeDistanceBetweenPointsAlongRoad使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类WayPoints
的用法示例。
在下文中一共展示了WayPoints::computeDistanceBetweenPointsAlongRoad方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: shortCut
/**
* @brief Check if some point ahead on the road is closer (L2) than along the road, to take a shortcut
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::shortCut(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Compute distances from robot to all points ahead. If any of them is laser-visible and significantly shorter than de distance along de road, try it!
WayPoints::iterator robot = road.getIterToClosestPointToRobot();
WayPoints::iterator best = road.begin();
for (WayPoints::iterator it = robot + 1; it != road.end(); ++it)
{
//qDebug() << __FUNCTION__ << it->isVisible << (it->pos - robot->pos).norm2() << road.computeDistanceBetweenPointsAlongRoad(robot, it);
if ( it->isVisible )
{
if (road.computeDistanceBetweenPointsAlongRoad(robot, it) - (it->pos - robot->pos).norm2() > 300) //Half robot SACARRRR
{
qDebug() << __FUNCTION__ << "Candidato";
//Check if the robot passes through the straight line
if (checkCollisionAlongRoad(innermodel, laserData, road, robot, it, ROBOT_RADIUS))
{
//Is so remove all intermadiate points between robot and new subtarget
qDebug() << __FUNCTION__ << "Confirmado";
best = it;
}
}
}
else
break;
}
if (best != road.begin() and (robot + 1) != road.end())
road.erase(robot + 1, best);
return false;
}