Angel 3.2
A 2D Game Prototyping Engine
PathFinder.cpp
1 
2 // Copyright (C) 2008-2013, Shane Liesegang
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of the copyright holder nor the names of any
14 // contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
29 
30 #include "stdafx.h"
31 #include "../AI/PathFinder.h"
32 
33 #include "../AI/SpatialGraph.h"
34 #include "../Infrastructure/TextRendering.h"
35 #include "../Util/DrawUtil.h"
36 #include "../Util/StringUtil.h"
37 #include "../Util/MathUtil.h"
38 
39 bool PathFinder::_drawPaths = false;
40 
41 PathFinder::PathFinder()
42 : _currentPathIndex(-1)
43 , _currentState(PFMS_START)
44 , _arrivalDist(0.0f)
45 {
46  InitializeStates();
47 }
48 
49 PathFinder::~PathFinder()
50 {
51  for( int i = 0; i < PFMS_COUNT; i++ )
52  {
53  delete _states[i];
54  }
55 
56 }
57 
58 
59 void PathFinder::FindNextMove( const Vector2& from, const Vector2& to, float arrivalDist, PathFinderMove& move )
60 {
61  _currentPos = from;
62  _currentDest = to;
63  _arrivalDist = arrivalDist;
64 
65  move.LastResult = PathFinder::PFMR_PATH_FOUND;
66 
67  while(!GetCurrentState()->Update( move ) );
68 }
69 
70 
71 void PathFinder::Render()
72 {
73  if( !_drawPaths )
74  return;
75  if( _currentPath.size() > 0 )
76  {
77  Vector2 lastPt;
78 
79  glColor3f(0,0,1.f);
80  for ( unsigned int i = 0; i < _currentPath.size(); i++ )
81  {
82  Vector2 curPt = _currentPath[i];
83  if( lastPt != Vector2(0) )
84  {
85  DrawLine( curPt, lastPt );
86  }
87  lastPt = curPt;
88  }
89 
90  if( _currentPathIndex >= 0 && _currentPathIndex < (int)_currentPath.size() )
91  {
92  glColor3f(1,0,1.f);
93 
94  DrawLine( _currentPath[_currentPathIndex], _currentPos );
95  DrawPoint( _currentPath[_currentPathIndex], 0.1f );
96  }
97  DrawPoint( _currentPath[_currentPath.size()-1], 0.1f );
98  }
99 
100  String currentState = GetCurrentState()->GetName();
101 
102  Vector2 screenCenter = MathUtil::WorldToScreen( _currentPos.X, _currentPos.Y );
103  //Print some vals
104  glColor3f(0,1.f,1.f);
105  DrawGameText( currentState + String(" ") + IntToString(_currentPathIndex), "ConsoleSmall", (int)screenCenter.X, (int)screenCenter.Y );
106 
107 }
108 
109 void PathFinder::EnableDrawPaths(bool enable)
110 {
111  _drawPaths = enable;
112 }
113 
114 
116 {
117 public:
118  virtual ~StartMoveState() {}
119  virtual const char* GetName() {return "Start";}
120  virtual bool Update( PathFinderMove& move )
121  {
122  if( !theSpatialGraph.IsInPathableSpace( GetCurrentPosition() ) )
123  {
124  SetNewState(PathFinder::PFMS_STARTRECOVER);
125  return true;
126  }
127  //find path
128  GetCurrentPath().clear();
129  bool retVal = theSpatialGraph.GetPath( GetCurrentPosition(), GetCurrentDestination(), GetCurrentPath() );
130 
131  if( retVal )
132  {
133  move.LastResult = PathFinder::PFMR_PATH_FOUND;
134  //Initialize path index
135  GetCurrentPathIndex() = 0;
136 
137  SetNewState( PathFinder::PFMS_FOLLOW );
138  //Keep ticking
139  return true;
140  }
141  else
142  {
143  move.LastResult = PathFinder::PFMR_PATH_NOT_FOUND;
144  }
145 
146  //We're done if path failed
147  return false;
148  }
149 
150 };
151 
153 {
154 public:
155  virtual ~ValidateMoveState() {}
156  virtual const char* GetName() {return "Validate";}
157  virtual bool Update( PathFinderMove& /*move*/ )
158  {
159  //Make sure we can keep heading to our current subgoal
160  const Vector2List& currentPath = GetCurrentPath();
161  int currentPathIndex = GetCurrentPathIndex();
162 
163  //has the destination changed
164  Vector2 vDest = currentPath[currentPath.size()-1];
165  if( vDest == GetCurrentDestination() )
166  {
167  if( theSpatialGraph.CanGo( GetCurrentPosition(), currentPath[currentPathIndex] ) )
168  {
169  SetNewState( PathFinder::PFMS_FOLLOW );
170  return true;
171  }
172  else if( !theSpatialGraph.CanGo( GetCurrentPosition(), GetCurrentPosition() ) )
173  {
174  //are we blocked
175  SetNewState( PathFinder::PFMS_RECOVER );
176  return true;
177  }
178  }
179 
180  //otherwise, try again
181  SetNewState( PathFinder::PFMS_START );
182  return true;
183  }
184 };
185 
187 {
188 public:
189  virtual ~FollowMoveState() {}
190  virtual const char* GetName() {return "Follow";}
191  virtual bool Update( PathFinderMove& move )
192  {
193  //check our current path index
194  const int iLookAheadCount = 3;
195 
196  const Vector2List& currentPath = GetCurrentPath();
197  int nextPathIndex = GetCurrentPathIndex();
198 
199  //Are we at our goal index
200  if( nextPathIndex == (int)currentPath.size() - 1 )
201  {
202  move.NextSubgoalPos = currentPath[nextPathIndex];
203  //check distance to goal
204  float sqDist = Vector2::DistanceSquared( GetCurrentPosition(), move.NextSubgoalPos );
205  float arrivalDistSq = GetCurrentArrivalDist();
206  arrivalDistSq *= arrivalDistSq;
207  if( sqDist <= arrivalDistSq )
208  {
209  //don't set move dir (we've arrived)
210  move.LastResult = PathFinder::PFMR_ARRIVED;
211  SetNewState( PathFinder::PFMS_VALIDATE );
212  return false;
213  }
214  }
215  else
216  {
217  //otherwise, see if we can advance our next subgoal
218  for( int i = 0 ; i < iLookAheadCount && (nextPathIndex+1) < (int)currentPath.size(); i++ )
219  {
220  if( theSpatialGraph.CanGo( GetCurrentPosition(), currentPath[nextPathIndex+1] ) )
221  {
222  ++nextPathIndex;
223  }
224  }
225 
226  GetCurrentPathIndex() = nextPathIndex;
227  move.NextSubgoalPos = currentPath[nextPathIndex];
228  }
229 
230  //Move dir is normalized towards next subgoal
231  move.MoveDir = Vector2::Normalize( move.NextSubgoalPos - GetCurrentPosition() );
232  //we're done this round
233  SetNewState( PathFinder::PFMS_VALIDATE );
234  return false;
235  }
236 
237 };
238 
240 {
241 public:
242  virtual ~RecoverMoveState() {}
243  virtual const char* GetName() {return "Recover";}
244  virtual bool Update( PathFinderMove& move )
245  {
246  //are we back in pathable space?
247  if( theSpatialGraph.IsInPathableSpace( GetCurrentPosition() ) )
248  {
249  SetNewState( PathFinder::PFMS_FOLLOW );
250  return true;
251  }
252 
253  const Vector2List& currentPath = GetCurrentPath();
254  int currentPathIndex = GetCurrentPathIndex();
255 
256  //otherwise, head toward our current pathnode
257  move.MoveDir = Vector2::Normalize( currentPath[currentPathIndex] - GetCurrentPosition() );
258 
259  return false;
260  }
261 };
262 
264 {
265 public:
266  virtual ~StartRecoverMoveState() {}
267  virtual const char* GetName() {return "StartRecover";}
268  virtual bool Update( PathFinderMove& move )
269  {
270  //are we back in pathable space?
271  if( theSpatialGraph.IsInPathableSpace( GetCurrentPosition() ) )
272  {
273  SetNewState( PathFinder::PFMS_START );
274  return true;
275  }
276 
277  //find the nearest non-blocked neighbor I can move to
278  Vector2 vGoTo;
279  if( theSpatialGraph.FindNearestNonBlocked( GetCurrentPosition(), vGoTo) )
280  {
281  move.MoveDir = Vector2::Normalize( vGoTo - GetCurrentPosition() );
282  }
283  else
284  {
285  move.LastResult = PathFinder::PFMR_PATH_NOT_FOUND;
286  }
287 
288  return false;
289  }
290 };
291 
292 void PathFinder::InitializeStates()
293 {
294  //Init states
295  _states[PFMS_START] = new StartMoveState();
296  _states[PFMS_START]->Initialize( this );
297 
298  _states[PFMS_VALIDATE] = new ValidateMoveState();
299  _states[PFMS_VALIDATE]->Initialize( this );
300 
301  _states[PFMS_FOLLOW] = new FollowMoveState();
302  _states[PFMS_FOLLOW]->Initialize( this );
303 
304  _states[PFMS_RECOVER] = new RecoverMoveState();
305  _states[PFMS_RECOVER]->Initialize( this );
306 
307  _states[PFMS_STARTRECOVER] = new StartRecoverMoveState();
308  _states[PFMS_STARTRECOVER]->Initialize( this );
309 
310 
311 }
312 
313 
314 FindNextMoveState* PathFinder::GetCurrentState()
315 {
316  return _states[_currentState];
317 }
318 void PathFinder::SetNewState( ePFMoveState newState )
319 {
320  //ignore gotos to the same state
321  if( newState == _currentState )
322  return;
323 
324  if( newState < 0 || newState >= PFMS_COUNT )
325  return;
326 
327  _currentState = newState;
328 }
329 
330 
331 void FindNextMoveState::SetNewState( PathFinder::ePFMoveState newState )
332 {
333  _pathFinder->SetNewState( newState );
334 }