src/genericgui/LinkAStar.cxx

   1 // Copyright (C) 2006-2022  CEA/DEN, EDF R&D
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19
  20 #include "LinkAStar.hxx"
  21
  22 #include <map>
  23 #include <list>
  24 #include <cmath>
  25
  26 //#define _DEVDEBUG_
  27 #include "YacsTrace.hxx"
  28
  29 using namespace std;
  30 using namespace YACS::HMI;
  31
  32
  33 LCostNode::LCostNode() : _gCost(0), _hCost(0), _fCost(0),
  34                          _parent( std::pair<int,int>(0,0) )
  35 {
  36   //DEBTRACE("LCostNode::LCostNode()");
  37 }
  38
  39 LCostNode::LCostNode(std::pair<int,int> parent) : _gCost(0), _hCost(0), _fCost(0),
  40                                                   _parent(parent)
  41 {
  42   //DEBTRACE("LCostNode::LCostNode(std::pair<int,int> parent)");
  43 }
  44
  45
  46
  47 LinkAStar::LinkAStar(const LinkMatrix& linkMatrix) : _linkMatrix(linkMatrix), _from(0,0), _to(0,0)
  48 {
  49   _closedList.clear();
  50   _openList.clear();
  51   _pq=std::priority_queue<Cost>();
  52 }
  53
  54 LinkAStar::~LinkAStar()
  55 {
  56 }
  57
  58 bool LinkAStar::computePath(LNode from, LNode to)
  59 {
  60   _closedList.clear();
  61   _openList.clear();
  62   _pq=std::priority_queue<Cost>();
  63   _from = from;
  64   _to = to;
  65   bool isPath = false;
  66
  67   //pair<int, int> curPos(0, 0);
  68   //LCostNode startCost(curPos);
  69
  70   pair<int, int> curPos = _from.getPos();
  71   LCostNode startCost(curPos);
  72   _openList[curPos] = startCost;
  73   moveToClosedList(curPos);
  74   addNeighbours(curPos);
  75
  76   while (! ((curPos.first == _to.getX()) && (curPos.second == _to.getY()))
  77          && (!_openList.empty()))
  78     {
  79       curPos = bestNode(_openList);
  80       moveToClosedList(curPos);
  81       DEBTRACE("curPos(" << curPos.first << "," << curPos.second << ")");
  82       addNeighbours(curPos);
  83     }
  84
  85   if ((curPos.first == _to.getX()) && (curPos.second == _to.getY()))
  86     isPath = true;
  87
  88   return isPath;
  89 }
  90
  91 LNodePath LinkAStar::givePath()
  92 {
  93   LNodePath aPath;
  94   aPath.clear();
  95   aPath.push_front(_to);
  96
  97   LNode current = _to;
  98   while (! current.isEqual(_from))
  99     {
 100       current = LNode(_closedList[current.getPos()].getParent());
 101       aPath.push_front(current);
 102       DEBTRACE("(" << current.getX() << "," << current.getY() << ")");
 103     }
 104 //   aPath.push_front(_from);
 105 //   DEBTRACE("(" << _from.getX() << "," << _from.getY() << ")");
 106   return aPath;
 107 }
 108
 109 bool LinkAStar::isAlreadyInList(std::pair<int,int> n, const LNodeMap& aList)
 110 {
 111   LNodeMap::const_iterator it = aList.find(n);
 112   if (it == aList.end())
 113     return false;
 114   else
 115     return true;
 116 }
 117
 118 void LinkAStar::addNeighbours(std::pair<int,int> currentNode)
 119 {
 120   LCostNode tmp(currentNode);
 121   int x = currentNode.first;
 122   int y = currentNode.second;
 123   for (int i = x-1; i <= x+1; i++)
 124     {
 125       if ((i<0) || (i >= _linkMatrix.imax()))
 126         continue;   // --- skip: outside matrix
 127       for (int j = y-1; j <= y+1; j++)
 128         {
 129           if ((j<0) || (j >= _linkMatrix.jmax()))
 130             continue; // --- skip: outside matrix
 131
 132           if ((i == x) && (j == y))
 133             continue; // --- skip: current node
 134
 135           if ((i != x) && (j != y))
 136             continue; // --- skip: diagonals (move only vertical or horizontal)
 137
 138           int cost = _linkMatrix.cost(i,j);
 139           if (! cost)
 140             continue; // --- skip: blocked
 141
 142           pair<int,int> pos(i,j);
 143           if (isAlreadyInList(pos, _closedList))
 144             continue; // --- skip: already in closed list
 145
 146           tmp.setGCost(_closedList[currentNode].getGCost() + cost*distance(x, y, i, j));
 147           tmp.setHCost(distance(i, j, _to.getX(), _to.getY()));
 148           tmp.setFCost(tmp.getGCost() + tmp.getHCost());
 149           if (isAlreadyInList(pos, _openList))
 150             {
 151               if (tmp.getFCost() < _openList[pos].getFCost())
 152                 {
 153                   _openList[pos] = tmp; // --- new path better, update node
 154                   _pq.push(Cost(tmp.getFCost(),pos));
 155                 }
 156             }
 157           else
 158             {
 159               _openList[pos] = tmp; // --- add node
 160               _pq.push(Cost(tmp.getFCost(),pos));
 161             }
 162         }
 163     }
 164 }
 165
 166 std::pair<int,int> LinkAStar::bestNode(const LNodeMap& aList)
 167 {
 168   std::pair<int, int> pos;
 169   do
 170     {
 171       pos=_pq.top().pos;
 172       _pq.pop();
 173     }
 174   while(aList.count(pos)==0);
 175   return pos;
 176 }
 177
 178 void LinkAStar::moveToClosedList(std::pair<int,int> pos)
 179 {
 180   _closedList[pos] = _openList[pos];
 181   if (_openList.erase(pos) == 0)
 182     DEBTRACE("node not in open list, can't delete");
 183 }