00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef STL_ASTAR_H
00027 #define STL_ASTAR_H
00028
00029
00030 #include <iostream>
00031 #include <stdio.h>
00032
00033 #include <assert.h>
00034
00035
00036 #include <algorithm>
00037 #include <set>
00038 #include <vector>
00039
00040 using namespace std;
00041
00042
00043 #include "fsa.h"
00044
00045
00046
00047 #define USE_FSA_MEMORY 1
00048
00049
00050
00051 #pragma warning( disable : 4786 )
00052
00053
00057 const int MAX_SEARCH_NODES = 1000;
00058
00059
00060 template <class UserState> class AStarSearch
00061 {
00062
00063 public:
00064
00065 enum
00066 {
00067 SEARCH_STATE_NOT_INITIALISED,
00068 SEARCH_STATE_SEARCHING,
00069 SEARCH_STATE_SUCCEEDED,
00070 SEARCH_STATE_FAILED,
00071 SEARCH_STATE_OUT_OF_MEMORY,
00072 SEARCH_STATE_INVALID
00073 };
00074
00075
00076
00077
00078
00079 public:
00080
00081 class Node
00082 {
00083 public:
00084
00085 Node *parent;
00086 Node *child;
00087
00088 float g;
00089 float h;
00090 float f;
00091
00092 Node() :
00093 parent( 0 ),
00094 child( 0 ),
00095 g( 0.0f ),
00096 h( 0.0f ),
00097 f( 0.0f )
00098 {
00099 }
00100
00101 UserState m_UserState;
00102 };
00103
00104
00105
00106
00107
00108 class HeapCompare_f
00109 {
00110 public:
00111
00112 bool operator() ( const Node *x, const Node *y ) const
00113 {
00114 return x->f > y->f;
00115 }
00116 };
00117
00118
00119 public:
00120
00121
00122
00123 AStarSearch( int MaxNodes = MAX_SEARCH_NODES ) :
00124 m_AllocateNodeCount(0),
00125 #if USE_FSA_MEMORY
00126 m_FixedSizeAllocator( MaxNodes ),
00127 #endif
00128 m_State( SEARCH_STATE_NOT_INITIALISED ),
00129 m_CurrentSolutionNode( NULL ),
00130 m_CancelRequest( false )
00131 {
00132 }
00133
00134
00135 void CancelSearch()
00136 {
00137 m_CancelRequest = true;
00138 }
00139
00140
00141 void SetStartAndGoalStates( UserState &Start, UserState &Goal )
00142 {
00143 m_CancelRequest = false;
00144
00145 m_Start = AllocateNode();
00146 m_Goal = AllocateNode();
00147
00148 assert((m_Start != NULL && m_Goal != NULL));
00149
00150 m_Start->m_UserState = Start;
00151 m_Goal->m_UserState = Goal;
00152
00153 m_State = SEARCH_STATE_SEARCHING;
00154
00155
00156
00157
00158 m_Start->g = 0;
00159 m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
00160 m_Start->f = m_Start->g + m_Start->h;
00161 m_Start->parent = 0;
00162
00163
00164
00165 m_OpenList.push_back( m_Start );
00166
00167
00168 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00169
00170
00171 m_Steps = 0;
00172 }
00173
00174
00175 unsigned int SearchStep()
00176 {
00177
00178 assert( (m_State > SEARCH_STATE_NOT_INITIALISED) &&
00179 (m_State < SEARCH_STATE_INVALID) );
00180
00181
00182 if( (m_State == SEARCH_STATE_SUCCEEDED) ||
00183 (m_State == SEARCH_STATE_FAILED)
00184 )
00185 {
00186 return m_State;
00187 }
00188
00189
00190
00191
00192 if( m_OpenList.empty() || m_CancelRequest )
00193 {
00194 FreeAllNodes();
00195 m_State = SEARCH_STATE_FAILED;
00196 return m_State;
00197 }
00198
00199
00200 m_Steps ++;
00201
00202
00203 Node *n = m_OpenList.front();
00204 pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00205 m_OpenList.pop_back();
00206
00207
00208 if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
00209 {
00210
00211
00212 m_Goal->parent = n->parent;
00213
00214
00215
00216 if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) )
00217 {
00218 FreeNode( n );
00219
00220
00221 Node *nodeChild = m_Goal;
00222 Node *nodeParent = m_Goal->parent;
00223
00224 do
00225 {
00226 nodeParent->child = nodeChild;
00227
00228 nodeChild = nodeParent;
00229 nodeParent = nodeParent->parent;
00230
00231 }
00232 while( nodeChild != m_Start );
00233
00234 }
00235
00236
00237 FreeUnusedNodes();
00238
00239 m_State = SEARCH_STATE_SUCCEEDED;
00240
00241 return m_State;
00242 }
00243 else
00244 {
00245
00246
00247
00248
00249
00250 m_Successors.clear();
00251
00252
00253
00254 bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL );
00255
00256 if( !ret )
00257 {
00258
00259 typename vector< Node * >::iterator successor;
00260
00261
00262 for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
00263 {
00264 FreeNode( (*successor) );
00265 }
00266
00267 m_Successors.clear();
00268
00269
00270 FreeAllNodes();
00271
00272 m_State = SEARCH_STATE_OUT_OF_MEMORY;
00273 return m_State;
00274 }
00275
00276
00277 for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
00278 {
00279
00280
00281 float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState );
00282
00283
00284
00285
00286
00287
00288
00289 typename vector< Node * >::iterator openlist_result;
00290
00291 for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ )
00292 {
00293 if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
00294 {
00295 break;
00296 }
00297 }
00298
00299 if( openlist_result != m_OpenList.end() )
00300 {
00301
00302
00303
00304 if( (*openlist_result)->g <= newg )
00305 {
00306 FreeNode( (*successor) );
00307
00308
00309 continue;
00310 }
00311 }
00312
00313 typename vector< Node * >::iterator closedlist_result;
00314
00315 for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ )
00316 {
00317 if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
00318 {
00319 break;
00320 }
00321 }
00322
00323 if( closedlist_result != m_ClosedList.end() )
00324 {
00325
00326
00327
00328 if( (*closedlist_result)->g <= newg )
00329 {
00330
00331 FreeNode( (*successor) );
00332
00333 continue;
00334 }
00335 }
00336
00337
00338
00339
00340 (*successor)->parent = n;
00341 (*successor)->g = newg;
00342 (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
00343 (*successor)->f = (*successor)->g + (*successor)->h;
00344
00345
00346
00347 if( closedlist_result != m_ClosedList.end() )
00348 {
00349
00350 FreeNode( (*closedlist_result) );
00351 m_ClosedList.erase( closedlist_result );
00352
00353
00354
00355
00356
00357
00358
00359 }
00360
00361
00362 if( openlist_result != m_OpenList.end() )
00363 {
00364
00365 FreeNode( (*openlist_result) );
00366 m_OpenList.erase( openlist_result );
00367
00368
00369
00370
00371
00372 make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00373
00374 }
00375
00376
00377 m_OpenList.push_back( (*successor) );
00378
00379
00380 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
00381
00382 }
00383
00384
00385
00386 m_ClosedList.push_back( n );
00387
00388 }
00389
00390 return m_State;
00391
00392 }
00393
00394
00395
00396 bool AddSuccessor( UserState &State )
00397 {
00398 Node *node = AllocateNode();
00399
00400 if( node )
00401 {
00402 node->m_UserState = State;
00403
00404 m_Successors.push_back( node );
00405
00406 return true;
00407 }
00408
00409 return false;
00410 }
00411
00412
00413
00414
00415 void FreeSolutionNodes()
00416 {
00417 Node *n = m_Start;
00418
00419 if( m_Start->child )
00420 {
00421 do
00422 {
00423 Node *del = n;
00424 n = n->child;
00425 FreeNode( del );
00426
00427 del = NULL;
00428
00429 } while( n != m_Goal );
00430
00431 FreeNode( n );
00432
00433 }
00434 else
00435 {
00436
00437
00438 FreeNode( m_Start );
00439 FreeNode( m_Goal );
00440 }
00441
00442 }
00443
00444
00445
00446
00447 UserState *GetSolutionStart()
00448 {
00449 m_CurrentSolutionNode = m_Start;
00450 if( m_Start )
00451 {
00452 return &m_Start->m_UserState;
00453 }
00454 else
00455 {
00456 return NULL;
00457 }
00458 }
00459
00460
00461 UserState *GetSolutionNext()
00462 {
00463 if( m_CurrentSolutionNode )
00464 {
00465 if( m_CurrentSolutionNode->child )
00466 {
00467
00468 Node *child = m_CurrentSolutionNode->child;
00469
00470 m_CurrentSolutionNode = m_CurrentSolutionNode->child;
00471
00472 return &child->m_UserState;
00473 }
00474 }
00475
00476 return NULL;
00477 }
00478
00479
00480 UserState *GetSolutionEnd()
00481 {
00482 m_CurrentSolutionNode = m_Goal;
00483 if( m_Goal )
00484 {
00485 return &m_Goal->m_UserState;
00486 }
00487 else
00488 {
00489 return NULL;
00490 }
00491 }
00492
00493
00494 UserState *GetSolutionPrev()
00495 {
00496 if( m_CurrentSolutionNode )
00497 {
00498 if( m_CurrentSolutionNode->parent )
00499 {
00500
00501 Node *parent = m_CurrentSolutionNode->parent;
00502
00503 m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
00504
00505 return &parent->m_UserState;
00506 }
00507 }
00508
00509 return NULL;
00510 }
00511
00512
00513
00514
00515 UserState *GetOpenListStart()
00516 {
00517 float f,g,h;
00518 return GetOpenListStart( f,g,h );
00519 }
00520
00521 UserState *GetOpenListStart( float &f, float &g, float &h )
00522 {
00523 iterDbgOpen = m_OpenList.begin();
00524 if( iterDbgOpen != m_OpenList.end() )
00525 {
00526 f = (*iterDbgOpen)->f;
00527 g = (*iterDbgOpen)->g;
00528 h = (*iterDbgOpen)->h;
00529 return &(*iterDbgOpen)->m_UserState;
00530 }
00531
00532 return NULL;
00533 }
00534
00535 UserState *GetOpenListNext()
00536 {
00537 float f,g,h;
00538 return GetOpenListNext( f,g,h );
00539 }
00540
00541 UserState *GetOpenListNext( float &f, float &g, float &h )
00542 {
00543 iterDbgOpen++;
00544 if( iterDbgOpen != m_OpenList.end() )
00545 {
00546 f = (*iterDbgOpen)->f;
00547 g = (*iterDbgOpen)->g;
00548 h = (*iterDbgOpen)->h;
00549 return &(*iterDbgOpen)->m_UserState;
00550 }
00551
00552 return NULL;
00553 }
00554
00555 UserState *GetClosedListStart()
00556 {
00557 float f,g,h;
00558 return GetClosedListStart( f,g,h );
00559 }
00560
00561 UserState *GetClosedListStart( float &f, float &g, float &h )
00562 {
00563 iterDbgClosed = m_ClosedList.begin();
00564 if( iterDbgClosed != m_ClosedList.end() )
00565 {
00566 f = (*iterDbgClosed)->f;
00567 g = (*iterDbgClosed)->g;
00568 h = (*iterDbgClosed)->h;
00569
00570 return &(*iterDbgClosed)->m_UserState;
00571 }
00572
00573 return NULL;
00574 }
00575
00576 UserState *GetClosedListNext()
00577 {
00578 float f,g,h;
00579 return GetClosedListNext( f,g,h );
00580 }
00581
00582 UserState *GetClosedListNext( float &f, float &g, float &h )
00583 {
00584 iterDbgClosed++;
00585 if( iterDbgClosed != m_ClosedList.end() )
00586 {
00587 f = (*iterDbgClosed)->f;
00588 g = (*iterDbgClosed)->g;
00589 h = (*iterDbgClosed)->h;
00590
00591 return &(*iterDbgClosed)->m_UserState;
00592 }
00593
00594 return NULL;
00595 }
00596
00597
00598
00599 int GetStepCount() { return m_Steps; }
00600
00601 void EnsureMemoryFreed()
00602 {
00603 #if USE_FSA_MEMORY
00604 assert(m_AllocateNodeCount == 0);
00605 #endif
00606
00607 }
00608
00609 private:
00610
00611
00612
00613 void FreeAllNodes()
00614 {
00615
00616 typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
00617
00618 while( iterOpen != m_OpenList.end() )
00619 {
00620 Node *n = (*iterOpen);
00621 FreeNode( n );
00622
00623 iterOpen ++;
00624 }
00625
00626 m_OpenList.clear();
00627
00628
00629 typename vector< Node * >::iterator iterClosed;
00630
00631 for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
00632 {
00633 Node *n = (*iterClosed);
00634 FreeNode( n );
00635 }
00636
00637 m_ClosedList.clear();
00638
00639
00640
00641 FreeNode(m_Goal);
00642 }
00643
00644
00645
00646
00647
00648 void FreeUnusedNodes()
00649 {
00650
00651 typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
00652
00653 while( iterOpen != m_OpenList.end() )
00654 {
00655 Node *n = (*iterOpen);
00656
00657 if( !n->child )
00658 {
00659 FreeNode( n );
00660
00661 n = NULL;
00662 }
00663
00664 iterOpen ++;
00665 }
00666
00667 m_OpenList.clear();
00668
00669
00670 typename vector< Node * >::iterator iterClosed;
00671
00672 for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
00673 {
00674 Node *n = (*iterClosed);
00675
00676 if( !n->child )
00677 {
00678 FreeNode( n );
00679 n = NULL;
00680
00681 }
00682 }
00683
00684 m_ClosedList.clear();
00685
00686 }
00687
00688
00689 Node *AllocateNode()
00690 {
00691
00692 #if !USE_FSA_MEMORY
00693 Node *p = new Node;
00694 return p;
00695 #else
00696 Node *address = m_FixedSizeAllocator.alloc();
00697
00698 if( !address )
00699 {
00700 return NULL;
00701 }
00702 m_AllocateNodeCount ++;
00703 Node *p = new (address) Node;
00704 return p;
00705 #endif
00706 }
00707
00708 void FreeNode( Node *node )
00709 {
00710
00711 m_AllocateNodeCount --;
00712
00713 #if !USE_FSA_MEMORY
00714 delete node;
00715 #else
00716 m_FixedSizeAllocator.free( node );
00717 #endif
00718 }
00719
00720 private:
00721
00722
00723 vector< Node *> m_OpenList;
00724
00725
00726 vector< Node * > m_ClosedList;
00727
00728
00729
00730 vector< Node * > m_Successors;
00731
00732
00733 unsigned int m_State;
00734
00735
00736 int m_Steps;
00737
00738
00739 Node *m_Start;
00740 Node *m_Goal;
00741
00742 Node *m_CurrentSolutionNode;
00743
00744 #if USE_FSA_MEMORY
00745
00746 FixedSizeAllocator<Node> m_FixedSizeAllocator;
00747 #endif
00748
00749
00750
00751 typename vector< Node * >::iterator iterDbgOpen;
00752 typename vector< Node * >::iterator iterDbgClosed;
00753
00754
00755 int m_AllocateNodeCount;
00756
00757 bool m_CancelRequest;
00758
00759 };
00760
00761
00762 #endif
00763