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) }