void MyastarPlanner::addNeighborCellsToOpenList(set<coupleOfCells> & OPL, vector <unsigned int> neighborCells, unsigned int parent, float gCostParent, unsigned int goalCell, double max) //,float tBreak)
{
set<coupleOfCells>::iterator it;
//vector <coupleOfCells> neighborsCellsOrdered;
for(uint i=0; i< neighborCells.size(); i++)
{
coupleOfCells CP;
CP.index=neighborCells[i]; //insert the neighbor cell
CP.parent=parent; //insert the parent cell
//calculate the gCost
CP.gCost=gCostParent+getMoveCost(parent,neighborCells[i]);
//calculate the hCost: Euclidian distance from the neighbor cell to the goalCell
CP.hCost=calculateHCost(neighborCells[i],goalCell);
//calculate fcost
double w=(CP.hCost/max)*3+1;
//double w=1;
CP.fCost=CP.gCost+w*CP.hCost;
// neighborsCellsOrdered.push_back(CP);
it = getPositionInList(OPL, CP.index);
if( it == openList.end() )
OPL.insert(CP);
else if (CP.fCost < it->fCost and CP.parent != it->parent){
OPL.erase(it);
OPL.insert(CP);
}
}
}
bool ThetaStar::findPath(Metrics& metrics)
{
if (_goal == _start)
{
return false;
}
_nrOfPathNodes = 0; //It's 1 because there's an offset in the loop later.
Vec2D currentPos = _start;
_grid[_start._x][_start._y]._open = 2;
while (currentPos != _goal) //loops until a path has been found
{
metrics.addExpandedNode(currentPos);
for (int i = 0; i < 8 && (_heuristicType != MANHATTAN || i < 4); i++) //Manhattan skips diagonals
{
Vec2D checkedPos = currentPos + NEIGHBOUR_OFFSETS[i];
if (isPositionValid(checkedPos) && _grid[checkedPos._x][checkedPos._y]._open != 2 && _grid[checkedPos._x][checkedPos._y]._traversable && //checks for borders and already visited
(_grid[checkedPos._x][currentPos._y]._traversable || _grid[currentPos._x][checkedPos._y]._traversable)) //checks for corners
{
bool openedBefore = true;
if (_grid[checkedPos._x][checkedPos._y]._open == 0) //check that node is not already in open list
{
calculateHCost(checkedPos); //As the program works now, h must be calculated before g.
openedBefore = false;
}
if (_grid[currentPos._x][currentPos._y]._parent != nullptr)
{
Vec2D parentPos = _grid[currentPos._x][currentPos._y]._parent->_position;
if (lineOfSightRay(parentPos, checkedPos))
{
calculateGCost(parentPos, checkedPos);
} else
{
calculateGCost(currentPos, checkedPos);
}
}
else
{
calculateGCost(currentPos, checkedPos);
}
if (!openedBefore && _grid[checkedPos._x][checkedPos._y]._open == 1) //Check that node was added to open list
{
metrics.addOpenedNode(checkedPos);
}
}
}
if (_openQueue.size() <= 0)
{
return false;
} else
{
currentPos = _openQueue.removeMin()->_position;
while (_grid[currentPos._x][currentPos._y]._open == 2)
{
if (_openQueue.size() <= 0)
{
return false;
}
currentPos = _openQueue.removeMin()->_position;
}
_grid[currentPos._x][currentPos._y]._open = 2;
}
}
while (currentPos != _start) //traces the route back to start
{
_nrOfPathNodes++;
currentPos = _grid[currentPos._x][currentPos._y]._parent->_position;
}
_path = new Vec2D[_nrOfPathNodes];
int c = 0;
currentPos = _goal;
while (currentPos != _start) //traces the route back to start
{
_path[c++] = currentPos;
currentPos = _grid[currentPos._x][currentPos._y]._parent->_position;
}
// _path[c++] = currentPos; //Excluding start position since it should already be known
metrics.setPathNodes(_path, _nrOfPathNodes, _grid[_goal._x][_goal._y]._gCost);
return true;
}