/**
* @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;
}
/**
* @brief Check if any of the waypoints has nan coordinates
*
* @param road ...
* @return bool
*/
bool ElasticBand::checkIfNAN(const WayPoints &road)
{
for (auto it = road.begin(); it != road.end(); ++it)
if (std::isnan(it->pos.x()) or std::isnan(it->pos.y()) or std::isnan(it->pos.z()))
{
road.print();
return true;
}
return false;
}