void TrajectoryFinder::init(StrategyMap* map, Tools::NGrid* grid, const Tools::RPoint &startPoint, int replanInterval,
int lostReplanInterval, double stopCircleRadius, double diagonalSmoothingMaxDistance, double turnAndMoveMinAngle)
{
_forcedNextDeplacementType = -1;
_isArrived = false;
_isBlocked = false;
_forceUnblock = true;
_hasObjective = false;
_forceStart = false;
_previousPoint = Tools::RPoint(-1000, -1000);
_manualAvoidingInProgress = false;
_pause = false;
_replanInterval = replanInterval;
_stopCircle = stopCircleRadius;
_diagSmoothingMaxDist = diagonalSmoothingMaxDistance;
_turnAndMoveMinAngle = turnAndMoveMinAngle;
_lostReplanTimer.setInterval(lostReplanInterval);
_map = map;
_deplacementType = Tools::TURN_THEN_MOVE;
_speed = 50;
_trajectory.clear();
_currentDestinations.clear();
_previousPoint = RPoint(-1000, -1000);
//int Pather
NGridNode* startNode = grid->getNearestNode(startPoint.toQPointF());
_pather->init(_map, grid, startNode);
}
Tools::RPoint NSParser::readFixedPoint(Symbol* symbol)
{
Tools::RPoint point;
bool xOk = false;
bool yOk = false;
if (symbol->type == NON_TERMINAL)
{
NonTerminal* nt = static_cast<NonTerminal*>(symbol);
for(Symbol* child: nt->children)
{
switch(child->symbolIndex)
{
case SYM_NUM:
case SYM_FIXED_ANGLE:
{
double value = readNum(child);
if (!xOk)
{
point.setX(value);
xOk = true;
}
else if (!yOk)
{
point.setY(value);
yOk = true;
}
else
{
double angle = readFixedAngleInRadian(child);
point.setTheta(angle);
}
break;
}
}
}
}
return point;
}
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;
}
QRectF NSParser::readFixedRect(Symbol* symbol, VariableList& variables)
{
QRectF r(0,0,1,1);
Tools::RPoint center;
bool definedByCenter = false;
double radius = 0;
int nbParamRead = 0;
if (symbol->type == NON_TERMINAL)
{
NonTerminal* nt = static_cast<NonTerminal*>(symbol);
for(Symbol* child: nt->children)
{
switch(child->symbolIndex)
{
case SYM_NUM:
{
double value = readNum(child);
if (definedByCenter)
radius = value;
else if (nbParamRead == 0)
r.setX(value);
else if (nbParamRead == 1)
r.setY(value);
else if (nbParamRead == 2)
r.setWidth(value);
else
r.setHeight(value);
++nbParamRead;
break;
}
case SYM_POINT:
case SYM_FIXED_POINT:
{
definedByCenter = true;
center = readPoint(child);
break;
}
case SYM_VAR:
{
QString name = readVar(child);
if (variables.contains(name))
{
DeclaredVariable& var = variables[name];
if (var.isPoint())
{
center = var.toPoint();
definedByCenter = true;
}
else
addError(NSParsingError::invalidVariableTypeError(name, "point", child));
}
else
addError(NSParsingError::undeclaredVariableError(name, child));
break;
}
}
}
}
if (definedByCenter)
{
r.setWidth(radius * 2.0);
r.setHeight(radius * 2.0);
r.moveCenter(center.toQPointF());
}
return r;
}
bool TrajectoryFinder::pointsEqualsInPather(const Tools::RPoint &p1, const Tools::RPoint &p2) const
{
return p1.isInCircle(p2, 50); //TODO, make 50 a parameter (minStopRadius)
}