1 // Copyright (C) 2006-2012 CEA/DEN, EDF R&D
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.
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.
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
17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
20 #include "LinkAStar.hxx"
27 #include "YacsTrace.hxx"
30 using namespace YACS::HMI;
33 LCostNode::LCostNode() : _gCost(0), _hCost(0), _fCost(0),
34 _parent( std::pair<int,int>(0,0) )
36 //DEBTRACE("LCostNode::LCostNode()");
39 LCostNode::LCostNode(std::pair<int,int> parent) : _gCost(0), _hCost(0), _fCost(0),
42 //DEBTRACE("LCostNode::LCostNode(std::pair<int,int> parent)");
47 LinkAStar::LinkAStar(const LinkMatrix& linkMatrix) : _linkMatrix(linkMatrix), _from(0,0), _to(0,0)
51 _pq=std::priority_queue<Cost>();
54 LinkAStar::~LinkAStar()
58 bool LinkAStar::computePath(LNode from, LNode to)
62 _pq=std::priority_queue<Cost>();
67 //pair<int, int> curPos(0, 0);
68 //LCostNode startCost(curPos);
70 pair<int, int> curPos = _from.getPos();
71 LCostNode startCost(curPos);
72 _openList[curPos] = startCost;
73 moveToClosedList(curPos);
74 addNeighbours(curPos);
76 while (! ((curPos.first == _to.getX()) && (curPos.second == _to.getY()))
77 && (!_openList.empty()))
79 curPos = bestNode(_openList);
80 moveToClosedList(curPos);
81 DEBTRACE("curPos(" << curPos.first << "," << curPos.second << ")");
82 addNeighbours(curPos);
85 if ((curPos.first == _to.getX()) && (curPos.second == _to.getY()))
91 LNodePath LinkAStar::givePath()
95 aPath.push_front(_to);
98 while (! current.isEqual(_from))
100 current = LNode(_closedList[current.getPos()].getParent());
101 aPath.push_front(current);
102 DEBTRACE("(" << current.getX() << "," << current.getY() << ")");
104 // aPath.push_front(_from);
105 // DEBTRACE("(" << _from.getX() << "," << _from.getY() << ")");
109 bool LinkAStar::isAlreadyInList(std::pair<int,int> n, const LNodeMap& aList)
111 LNodeMap::const_iterator it = aList.find(n);
112 if (it == aList.end())
118 void LinkAStar::addNeighbours(std::pair<int,int> currentNode)
120 LCostNode tmp(currentNode);
121 int x = currentNode.first;
122 int y = currentNode.second;
123 for (int i = x-1; i <= x+1; i++)
125 if ((i<0) || (i >= _linkMatrix.imax()))
126 continue; // --- skip: outside matrix
127 for (int j = y-1; j <= y+1; j++)
129 if ((j<0) || (j >= _linkMatrix.jmax()))
130 continue; // --- skip: outside matrix
132 if ((i == x) && (j == y))
133 continue; // --- skip: current node
135 if ((i != x) && (j != y))
136 continue; // --- skip: diagonals (move only vertical or horizontal)
138 int cost = _linkMatrix.cost(i,j);
140 continue; // --- skip: blocked
142 pair<int,int> pos(i,j);
143 if (isAlreadyInList(pos, _closedList))
144 continue; // --- skip: already in closed list
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))
151 if (tmp.getFCost() < _openList[pos].getFCost())
153 _openList[pos] = tmp; // --- new path better, update node
154 _pq.push(Cost(tmp.getFCost(),pos));
159 _openList[pos] = tmp; // --- add node
160 _pq.push(Cost(tmp.getFCost(),pos));
166 std::pair<int,int> LinkAStar::bestNode(const LNodeMap& aList)
168 std::pair<int, int> pos;
174 while(aList.count(pos)==0);
178 void LinkAStar::moveToClosedList(std::pair<int,int> pos)
180 _closedList[pos] = _openList[pos];
181 if (_openList.erase(pos) == 0)
182 DEBTRACE("node not in open list, can't delete");