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