HOG2
HPAStar.cpp
Go to the documentation of this file.
1 /*
2  * hpaStar.cpp
3  *
4  * Created by Renee Jansen on 06/16/06
5  *
6  */
7 
8 #include "HPAStar.h"
9 #include "AStar3.h"
10 
11 #include "AStar.h"
12 #include "Timer.h"
13 
14 using namespace GraphAbstractionConstants;
15 
16 const static int verbose = 0;
17 const int MAX_INT = 2147483647;
18 
19 hpaStar::hpaStar()
20 {
21  partialLimit = -1;
22  fromnum = 0;
23  tonum = 0;
24  smoothing = true;
25  smType = END;
26 
27  sprintf(algName,"HPA*(%d)",partialLimit);
28 }
29 
33 void hpaStar::setSmoothing(bool smooth){
34  smoothing = smooth;
35 }
36 
45 path* hpaStar::GetPath(GraphAbstraction *aMap, node* from, node* to, reservationProvider *)
46 {
47  // Make sure a cluster abstraction has been set & it's the same as the abstraction being passed
48  // in to this function
49  assert(m==aMap);
50 
51  if ((from==to) || (from ==0) || (to==0)){
52  if (verbose) std::cout<<"Returning an empty path\n";
53  return 0;
54  }
55 
56  fromnum = 0;
57  tonum = 0;
58 
59  Timer t;
60  //if the nodes are in the same cluster, just do A*
61  //this takes care of the case where both nodes are in some enclosed
62  //area inside a cluster
63 
64  if (m->getClusterIdFromNode(from)==m->getClusterIdFromNode(to))
65  {
66  aStarOld astar;
67  path* p = astar.GetPath(m,from,to);
68  nodesExpanded = astar.GetNodesExpanded();
69  nodesTouched = astar.GetNodesTouched();
70  if (verbose)std::cout<<"Nodes are inside the same cluster\n";
71  return p;
72  }
73 
74  setUpSearch(from, to);
75 
76  // Check if there's a path at all
77  if (!m->Pathable(from,to)){
78  if (verbose) std::cout<<"Not Pathable\n";
79  cleanUpSearch();
80  return 0;
81  }
82 
83  else
84  if (verbose) std::cout<<"Pathable\n";
85 
86  // t.StartTimer();
87  path* abPath = findAbstractPath(fromnum,tonum);
88  //std::cout<<"abstract path "<<t.EndTimer()<<std::endl;
89 
90  if (!abPath){
91  cleanUpSearch();
92  return 0;
93  }
94  path* mapPath = 0;
95  if (partialLimit > 0){
96  path *trav = abPath;
97  path *thisPart = new path(trav->n);
98  for (int x = 0; x < partialLimit-1; x++){
99  if (trav->next) {
100  trav = trav->next;
101  thisPart->tail()->next = new path(trav->n);
102  }
103  }
104 
105 
106  node* upper = trav->tail()->n;
107  point3d s(upper->GetLabelF(kXCoordinate),upper->GetLabelF(kYCoordinate),-1);
108  int px;
109  int py;
110  m->GetMap()->GetPointFromCoordinate(s,px,py);
111 
112  node* newTo = m->GetNodeFromMap(px,py);
113  mapPath = findMapPath(thisPart, from, newTo);
114  delete thisPart;
115  }
116  else
117  {
118  // t.StartTimer();
119  mapPath = findMapPath(abPath,from,to);
120  //std::cout<<"Found map path "<<t.EndTimer()<<std::endl;
121  }
122 
123  path* finalPath = 0;
124 
125  if (smoothing)
126  {
127  //t.StartTimer();
128  finalPath = smoothPath(mapPath);
129  //std::cout<<"Smoothing: "<<t.EndTimer()<<std::endl<<std::endl;
130  delete mapPath;
131  }
132  else {
133  finalPath = mapPath;
134  mapPath = 0;
135  }
136 
137  cleanUpSearch();
138 
139  delete abPath;
140 
141  return finalPath;
142 }
143 
144 /*
145  * setUpSearch sets the number of nodes expanded and touched to 0
146  * and inserts the start and goal nodes into the abstract Graph
147  */
148 void hpaStar::setUpSearch(node* from, node* to)
149 {
150  if (verbose) std::cout<<"HPA*: setting up search\n";
151  // set nodes expanded & nodes touched to 0
152  nodesExpanded = 0;
153  nodesTouched = 0;
154 
155  int touched =0;
156  int expanded = 0;
157  // insert the nodes
158  // Timer t;
159  // t.StartTimer();
160  fromnum = m->insertNode(from,expanded,touched);
161  nodesExpanded += expanded;
162  nodesTouched += touched;
163  tonum = m->insertNode(to,expanded,touched);
164  nodesExpanded += expanded;
165  nodesTouched += touched;
166  // std::cout<<"Time taken to insert: "<<t.EndTimer();
167 }
168 
169 /*
170  * finds a path on the abstract level of a map
171  */
172 path* hpaStar::findAbstractPath(node* from, node* to)
173 {
174  if (verbose) std::cout<<"HPA*: finding the abstract path\n";
175  aStarOld astar;
176  assert(from == fromnum);
177  assert(to == tonum);
178  path* p = astar.GetPath(m, fromnum, tonum);
179  nodesExpanded = astar.GetNodesExpanded();
180  nodesTouched = astar.GetNodesTouched();
181 
182  if (verbose){
183  std::cout<<"Abstract path: ";
184  if (p)
185  p->Print();
186  else
187  std::cout<<"Null";
188  std::cout<<std::endl;
189  }
190 
191  return (p);
192 }
193 
194 /*
195  * finds the map-level path corresponding to the abstract path
196  * It uses cached paths for each edge in the abstract path.
197  */
198 path* hpaStar::findMapPath(path* abPath, node* /*from*/,node* /*to*/)
199 {
200  Graph *g = m->GetAbstractGraph(1);
201  path* curr = abPath;
202 
203  path* returnme = 0;
204 
205  node* n = curr->n;
206  curr = curr->next;
207  node* n2 = curr->n;
208 
209  edge* e = g->FindEdge(n->GetNum(), n2->GetNum());
210 
211  path* lowlevel = m->getCachedPath(e);
212 
213  if (lowlevel)
214  { // edge was cached
215  if ((lowlevel->n == m->getLowLevelNode(n2)) &&
216  (lowlevel->tail()->n == m->getLowLevelNode(n)))
217  {
218  // The cached path is backwards - have to reverse it
219  lowlevel = lowlevel->reverse();
220  }
221 
222  returnme = lowlevel;
223  }
224  else {
225  returnme = new path(m->getLowLevelNode(n), new path(m->getLowLevelNode(n2)));
226  }
227 
228  while(curr->next){
229  n = n2;
230  curr = curr->next;
231  n2 = curr->n;
232 
233  /*
234  point3d s(n->GetLabelF(kXCoordinate),n->GetLabelF(kYCoordinate),-1);
235  int px;
236  int py;
237  m->GetMap()->GetPointFromCoordinate(s,px,py);
238 
239  point3d t(n2->GetLabelF(kXCoordinate),n2->GetLabelF(kYCoordinate),-1);
240 
241  m->GetMap()->GetPointFromCoordinate(t,px,py);
242  */
243 
244  e = g->FindEdge(n->GetNum(), n2->GetNum());
245 
246  lowlevel = m->getCachedPath(e);
247 
248 
249  if (lowlevel)
250  {
251 
252  if ((lowlevel->n == m->getLowLevelNode(n2)) &&
253  (lowlevel->tail()->n == m->getLowLevelNode(n)))
254  {
255  lowlevel = lowlevel->reverse();
256  }
257 
258  path *tailend = returnme->tail();
259  if (tailend->n == lowlevel->n)
260  {
261  tailend->next = lowlevel->next;
262  lowlevel->next = 0;
263  delete lowlevel;
264  }
265  else{
266  returnme->tail()->next = lowlevel;
267  }
268  }
269  else
270  {
271  returnme->tail()->next = new path(m->getLowLevelNode(n2));
272  }
273  }
274  return returnme;
275 }
276 
280 void hpaStar::cleanUpSearch()
281 {
282  // remove nodes from map
283  if ((fromnum!=0) && (tonum != 0)){
284  // Timer t;
285  //t.StartTimer();
286  m->removeNodes(fromnum,tonum);
287  //std::cout<<"It took "<<t.EndTimer()<<" to remove nodes\n";
288  }
289  fromnum = 0;
290  tonum = 0;
291 }
292 
299 path* hpaStar::smoothPath(path* p)
300 {
301  if (verbose) std::cout<<"Smoothing the path\n";
302 
303  // findMinMax(p);
304  int clusterSize = m->getClusterSize();
305 
306  // put the path nodes in a vector
307  lookup.clear();
308  int i=0;
309  //assert(pcopy);
310  for (path *tmp = p; tmp != 0; tmp = tmp->next)
311  {
312  lookup.push_back(tmp->n);
313  // set key=index in lookup
314  tmp->n->SetLabelL(kTemporaryLabel,i);
315  i++;
316  }
317 
318  unsigned int n = 0;
319  path* smooth = 0;
320 
321  while(true){
322  //Skip blanks
323  while(lookup[n]==0 && n<lookup.size()-1)
324  n++;
325 
326  if (n>=lookup.size()-1){
327  break;
328  }
329 
330  unsigned int last = lookup[n]->GetLabelL(kTemporaryLabel);
331 
332  // Node's temporary label != last --> this node occurs more than
333  // once. Cut out the in between path & go back to beginning of while loop
334  if (last!=n)
335  {
336  std::cout<<"in here\n";
337  for (unsigned int j=n; j<last && j<lookup.size(); j++)
338  {
339  lookup[j]=0;
340  }
341  n = last;
342  continue;
343  }
344 
345  int currX = lookup[n]->GetLabelL(kFirstData);
346  int currY = lookup[n]->GetLabelL(kFirstData+1);
347  minx = currX - clusterSize;
348  maxx = currX + clusterSize;
349  miny = currY - clusterSize;
350  maxy = currY + clusterSize;
351 
352  int dir;
353  for (dir = NORTH; dir <= NW; dir++)
354  {
355  // get a shortcut if it exists
356  path* pathToNode = nextPathNode(lookup[n],dir);
357 
358  // paste the shortcut into our current path
359  if (pathToNode)
360  {
361  int lastNode = pathToNode->tail()->n->GetLabelL(kTemporaryLabel);
362  int curr = pathToNode->n->GetLabelL(kTemporaryLabel);
363  if (lastNode > curr && !nextInLookup(lastNode, curr,lookup))
364  {
365  // make sure it's not the next one
366  unsigned int index = n;
367  path* pathCopy = pathToNode;
368  //path* backup = pathCopy;
369  unsigned int end = pathToNode->tail()->n->GetLabelL(kTemporaryLabel);
370 
371  while(pathCopy->next){
372  //make sure we're not overwriting anything
373  assert(index <= end);
374 
375  lookup[index]=pathCopy->n;
376  pathCopy->n->SetLabelL(kTemporaryLabel,index);
377  pathCopy = pathCopy->next;
378  index++;
379  }
380  assert(index <= end);
381 
382  lookup[index]=pathCopy->n;
383  pathCopy->n->SetLabelL(kTemporaryLabel,index);
384 
385  index++;
386 
387  while(index<=end){
388  lookup[index]= 0;
389  index++;
390  }
391 
392  if (smType==END){
393  n = end;
394  }
395  else if (smType==TWO_BACK)
396  n = backTwoNodes(end,lookup);
397  else
398  n++;
399 
400  delete pathToNode; pathToNode = 0;
401  //delete backup;
402  break;
403 
404  }
405  else if (dir==NW){
406  n++;
407  }
408  delete pathToNode;
409  }
410  else if (dir==NW){
411  n++;
412  }
413  } //end for every direction
414 
415 
416  }
417 
418  //Create smoothed path from lookup table
419  for (unsigned int j=0; j<lookup.size(); j++){
420  if (lookup[j]!=0){
421  if (!smooth)
422  smooth = new path(lookup[j],0);
423  else
424  smooth->tail()->next = new path(lookup[j],0);
425  }
426  }
427 
428  return smooth;
429 }
430 
434 int hpaStar::backTwoNodes(int i, std::vector<node*> lookupVec)
435 {
436  i--;
437  while(lookupVec[i]==NULL){
438  i--;
439  }
440  i--;
441  while(lookupVec[i]==NULL){
442  i--;
443  }
444  return i;
445 }
446 
447 
453 bool hpaStar::nextInLookup(int last, int curr, std::vector<node*> lookupVec)
454 {
455  if (last<curr)
456  return false;
457 
458  for (int i=curr+1; i<=last; i++) //
459  {
460  if (i==last)
461  return true;
462  if (lookupVec[i]!=NULL)
463  return false;
464  }
465  return true;
466 }
467 
473 path* hpaStar::nextPathNode(node* n, int dir)
474 {
475  Graph *g = m->GetAbstractGraph(0);
476 
477  int px = n->GetLabelL(kFirstData);
478  int py = n->GetLabelL(kFirstData+1);
479 
480  node* next = getNextNode(px,py,dir);
481 
482  if (next){
483  nodesTouched++;
484 
485  int nextKey = next->GetLabelL(kTemporaryLabel);
486  edge* e = g->FindEdge(n->GetNum(), next->GetNum());
487 
488  if (e && (nextKey >= 0) && (nextKey < static_cast<int>(lookup.size())) && (lookup[nextKey]==next)){
489  //we're done - we found the path
490  return new path(n,new path(next));
491  }
492  else{ // look further for a path node
493  if (e){
494  path * p = nextPathNode(next,dir);
495  if (p){
496  return new path(n,p);
497  }
498  else // haven't found a path node
499  return 0;
500  }
501  else // no edge here
502  return 0;
503  }
504  }
505  else{ // we won't hit the path
506  return 0;
507  }
508 }
509 
514 node* hpaStar::getNextNode(int x, int y, int dir)
515 {
516  if ((x < minx) || (x > maxx) || (y < miny) || (y>maxy))
517  return 0;
518 
519  switch(dir)
520  {
521  case NORTH:
522  return m->GetNodeFromMap(x,y+1);
523  break;
524  case EAST:
525  return m->GetNodeFromMap(x+1,y);
526  break;
527  case SOUTH:
528  return m->GetNodeFromMap(x, y-1);
529  break;
530  case WEST:
531  return m->GetNodeFromMap(x-1, y);
532  break;
533  case NE:
534  return m->GetNodeFromMap(x+1, y+1);
535  break;
536  case SE:
537  return m->GetNodeFromMap(x+1, y-1);
538  break;
539  case SW:
540  return m->GetNodeFromMap(x-1, y-1);
541  break;
542  case NW:
543  return m->GetNodeFromMap(x-1, y+1);
544  break;
545  default:
546  return 0;
547  }
548 }
549 
550 void hpaStar::findMinMax(path* p)
551 {
552  minx = MAX_INT;
553  miny = MAX_INT;
554  maxx = -1;
555  maxy = -1;
556 
557  path* pcopy = p;
558 
559  while(pcopy->next)
560  {
561  int x = pcopy->n->GetLabelL(kFirstData);
562  int y = pcopy->n->GetLabelL(kFirstData+1);
563 
564  if (x < minx) minx = x;
565  if (x > maxx) maxx = x;
566 
567  if (y < miny) miny = y;
568  if (y > maxy) maxy = y;
569 
570  pcopy = pcopy->next;
571  }
572 
573  int x = pcopy->n->GetLabelL(kFirstData);
574  int y = pcopy->n->GetLabelL(kFirstData+1);
575 
576  if (x < minx) minx = x;
577  if (x > maxx) maxx = x;
578 
579  if (y < miny) miny = y;
580  if (y > maxy) maxy = y;
581 
582  assert((minx != MAX_INT)&&(miny != MAX_INT)&&(maxx != -1)&&(maxy != -1));
583 
584 }
hpaStar::GetPath
virtual path * GetPath(GraphAbstraction *aMap, node *from, node *to, reservationProvider *rp=0)
Returns the HPA* path between from and to.
Definition: HPAStar.cpp:45
GraphAbstraction
A generic class for basic operations on Graph abstractions.
Definition: GraphAbstraction.h:63
GraphAbstractionConstants
Definition: GraphAbstraction.h:22
node::SetLabelL
void SetLabelL(unsigned int index, long val) const
Definition: Graph.cpp:702
MAX_INT
const int MAX_INT
Definition: HPAStar.cpp:17
aStarOld::GetPath
path * GetPath(GraphAbstraction *aMap, node *from, node *to, reservationProvider *rp=0)
Definition: AStar3.cpp:35
hpaStar::nextInLookup
bool nextInLookup(int last, int curr, std::vector< node * > lookup)
find out whether last is the next 'real' index in the lookup table after curr.
Definition: HPAStar.cpp:453
hpaStar::nextPathNode
path * nextPathNode(node *n, int dir)
shoot a ray in direction dir and see if you hit the path Return the better path if you find it; 0 if ...
Definition: HPAStar.cpp:473
Graph::FindEdge
edge * FindEdge(unsigned int from, unsigned int to)
Finds an edge between nodes with ids from and to, no matter which direction.
Definition: Graph.cpp:230
SearchAlgorithm::GetNodesTouched
uint64_t GetNodesTouched()
Definition: SearchAlgorithm.h:32
hpaStar::findMinMax
void findMinMax(path *p)
Definition: HPAStar.cpp:550
path::n
node * n
Definition: Path.h:22
hpaStar::smoothPath
path * smoothPath(path *p)
from HPA* smoothwizard.cpp
Definition: HPAStar.cpp:299
SearchAlgorithm::GetNodesExpanded
uint64_t GetNodesExpanded()
Definition: SearchAlgorithm.h:31
hpaStar::getNextNode
node * getNextNode(int x, int y, int dir)
get the next node from map coordinates (x,y) in direction dir.
Definition: HPAStar.cpp:514
AStar3.h
aStarOld
A sample A* implementation.
Definition: AStar3.h:26
Graph
A generic Graph class.
Definition: Graph.h:66
path::Print
void Print(bool beginning=true)
Definition: Path.cpp:44
Timer.h
GraphAbstractionConstants::kFirstData
@ kFirstData
Definition: GraphAbstraction.h:34
point3d
#define point3d
Definition: GLUtil.h:123
hpaStar::hpaStar
hpaStar()
Definition: HPAStar.cpp:19
hpaStar::backTwoNodes
int backTwoNodes(int i, std::vector< node * > lookup)
Find the index of the node two nodes back in the path.
Definition: HPAStar.cpp:434
hpaStar::findAbstractPath
path * findAbstractPath(node *from, node *to)
Definition: HPAStar.cpp:172
hpaStar::findMapPath
path * findMapPath(path *abPath, node *from, node *to)
Definition: HPAStar.cpp:198
GraphAbstractionConstants::kYCoordinate
@ kYCoordinate
Definition: GraphAbstraction.h:31
hpaStar::setSmoothing
void setSmoothing(bool smooth)
Set whether we want to do path smoothing.
Definition: HPAStar.cpp:33
hpaStar::setUpSearch
void setUpSearch(node *from, node *to)
Definition: HPAStar.cpp:148
path
std::vector< xyLoc > path
Definition: Sample.cpp:43
verbose
const static int verbose
Definition: HPAStar.cpp:16
GraphAbstractionConstants::kXCoordinate
@ kXCoordinate
Definition: GraphAbstraction.h:30
hpaStar::cleanUpSearch
void cleanUpSearch()
Remove the start & goal nodes from the Graph.
Definition: HPAStar.cpp:280
node::GetLabelF
double GetLabelF(unsigned int index) const
Definition: Graph.h:215
path::tail
path * tail()
Definition: Path.h:28
HPAStar.h
node::GetNum
unsigned int GetNum() const
Definition: Graph.h:176
GraphAbstractionConstants::kTemporaryLabel
@ kTemporaryLabel
Definition: GraphAbstraction.h:29
reservationProvider
Definition: ReservationProvider.h:33
Timer
Definition: Timer.h:19
path::next
path * next
Definition: Path.h:23
node::GetLabelL
long GetLabelL(unsigned int index) const
Definition: Graph.h:220
path
A linked list of nodes which form a continuous path.
Definition: Path.h:20
AStar.h
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
path::reverse
path * reverse()
reverses path in place, and returns pointer to new head of path
Definition: Path.cpp:62
edge
Edge class for connections between node in a Graph.
Definition: Graph.h:129
Generated by   doxygen 1.8.17