bool TrajectoryFinder::isArrived(const Tools::RPoint &point) const
{
if (!_hasObjective || _currentDestinations.isEmpty())
{
return false;
}
if ((_isPureRotation && _mouvementType == Tools::TOURNE_VERS_XY) || (_mouvementType == Tools::TOURNE_VERS_XY_AR))
{
if (fabs(point.angle(_currentDestinations.first()) - point.getTheta()) < 0.2)
return true;
}
if (_isPureRotation && fabs(point.diffAngle(_currentDestinations.first())) < 0.2)
return true;
if (!_isPureRotation && point.isInCircle(_currentDestinations.first(), 40))
return true;
return false;
}
bool TrajectoryFinder::pointsEqualsInPather(const Tools::RPoint &p1, const Tools::RPoint &p2) const
{
return p1.isInCircle(p2, 50); //TODO, make 50 a parameter (minStopRadius)
}