Beispiel #1
0
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;
}
Beispiel #2
0
bool TrajectoryFinder::pointsEqualsInPather(const Tools::RPoint &p1, const Tools::RPoint &p2) const
{
	return p1.isInCircle(p2, 50); //TODO, make 50 a parameter (minStopRadius)
}