src/genericgui/LinkAStar.cxx

   1 //  Copyright (C) 2006-2008  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.
   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 #include "LinkAStar.hxx"
  20
  21 #include <map>
  22 #include <list>
  23 #include <cmath>
  24
  25 //#define _DEVDEBUG_
  26 #include "YacsTrace.hxx"
  27
  28 using namespace std;
  29 //using namespace YACS::ENGINE;
  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
  52 }
  53
  54 LinkAStar::~LinkAStar()
  55 {
  56 }
  57
  58 bool LinkAStar::computePath(LNode from, LNode to)
  59 {
  60   _closedList.clear();
  61   _openList.clear();
  62   _from = from;
  63   _to = to;
  64   bool isPath = false;
  65
  66   //pair<int, int> curPos(0, 0);
  67   //LCostNode startCost(curPos);
  68
  69   pair<int, int> curPos = _from.getPos();
  70   LCostNode startCost(curPos);
  71   _openList[curPos] = startCost;
  72   moveToClosedList(curPos);
  73   addNeighbours(curPos);
  74
  75   while (! ((curPos.first == _to.getX()) && (curPos.second == _to.getY()))
  76          && (!_openList.empty()))
  77     {
  78       curPos = bestNode(_openList);
  79       moveToClosedList(curPos);
  80       DEBTRACE("curPos(" << curPos.first << "," << curPos.second << ")");
  81       addNeighbours(curPos);
  82     }
  83
  84   if ((curPos.first == _to.getX()) && (curPos.second == _to.getY()))
  85     isPath = true;
  86
  87   return isPath;
  88 }
  89
  90 LNodePath LinkAStar::givePath()
  91 {
  92   LNodePath aPath;
  93   aPath.clear();
  94   aPath.push_front(_to);
  95
  96   LNode current = _to;
  97   while (! current.isEqual(_from))
  98     {
  99       current = LNode(_closedList[current.getPos()].getParent());
 100       aPath.push_front(current);
 101       DEBTRACE("(" << current.getX() << "," << current.getY() << ")");
 102     }
 103 //   aPath.push_front(_from);
 104 //   DEBTRACE("(" << _from.getX() << "," << _from.getY() << ")");
 105   return aPath;
 106 }
 107
 108 bool LinkAStar::isAlreadyInList(std::pair<int,int> n, const LNodeMap& aList)
 109 {
 110   LNodeMap::const_iterator it = aList.find(n);
 111   if (it == aList.end())
 112     return false;
 113   else
 114     return true;
 115 }
 116
 117 void LinkAStar::addNeighbours(std::pair<int,int> currentNode)
 118 {
 119   LCostNode tmp(currentNode);
 120   int x = currentNode.first;
 121   int y = currentNode.second;
 122   for (int i = x-1; i <= x+1; i++)
 123     {
 124       if ((i<0) || (i >= _linkMatrix.imax()))
 125         continue;   // --- skip: outside matrix
 126       for (int j = y-1; j <= y+1; j++)
 127         {
 128           if ((j<0) || (j >= _linkMatrix.jmax()))
 129             continue; // --- skip: outside matrix
 130
 131           if ((i == x) && (j == y))
 132             continue; // --- skip: current node
 133
 134           if ((i != x) && (j != y))
 135             continue; // --- skip: diagonals (move only vertical or horizontal)
 136
 137           int cost = _linkMatrix.cost(i,j);
 138           if (! cost)
 139             continue; // --- skip: blocked
 140
 141           pair<int,int> pos(i,j);
 142           if (isAlreadyInList(pos, _closedList))
 143             continue; // --- skip: already in closed list
 144
 145           tmp.setGCost(_closedList[currentNode].getGCost() + cost*distance(x, y, i, j));
 146           tmp.setHCost(distance(i, j, _to.getX(), _to.getY()));
 147           tmp.setFCost(tmp.getGCost() + tmp.getHCost());
 148           if (isAlreadyInList(pos, _openList))
 149             {
 150               if (tmp.getFCost() < _openList[pos].getFCost())
 151                 {
 152                   _openList[pos] = tmp; // --- new path better, update node
 153                 }
 154             }
 155           else
 156             {
 157               _openList[pos] = tmp; // --- add node
 158             }
 159         }
 160     }
 161 }
 162
 163 std::pair<int,int> LinkAStar::bestNode(const LNodeMap& aList)
 164 {
 165   double fCost = (aList.begin()->second).getFCost();
 166   pair<int, int> pos = aList.begin()->first;
 167   for (LNodeMap::const_iterator it = aList.begin(); it != aList.end(); ++it)
 168     if ((it->second).getFCost() < fCost)
 169       {
 170         fCost = (it->second).getFCost();
 171         pos = it->first;
 172       }
 173   return pos;
 174 }
 175
 176 void LinkAStar::moveToClosedList(std::pair<int,int> pos)
 177 {
 178   _closedList[pos] = _openList[pos];
 179   if (_openList.erase(pos) == 0)
 180     DEBTRACE("node not in open list, can't delete");
 181 }
 182
 183 double LinkAStar::distance(int i1, int j1, int i2, int j2)
 184 {
 185   return sqrt(double((i1-i2)*(i1-i2) + (j1-j2)*(j1-j2)));
 186 }