Angel 3.2
A 2D Game Prototyping Engine
SpatialGraph.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/SpatialGraph.h"
32 
33 #include "../Infrastructure/TextRendering.h"
34 #include "../Infrastructure/World.h"
35 #include "../AI/Ray2.h"
36 #include "../Util/DrawUtil.h"
37 #include "../Util/StringUtil.h"
38 #include "../Util/MathUtil.h"
39 
40 #include <Box2D/Box2D.h>
41 
42 void SpatialGraphKDNode::Render()
43 {
44  if( HasChildren() )
45  {
46  LHC->Render();
47  RHC->Render();
48  return;
49  }
50 
51  if( theSpatialGraph.GetDrawBlocked() )
52  {
53  if( bBlocked )
54  {
55  glColor4f(1,0,0,0.25f);
56  BBox.RenderBox();
57  }
58  }
59 
60  if( theSpatialGraph.GetDrawBounds() )
61  {
62  glColor4f(0,0,0,1.f);
63  BBox.RenderOutline();
64  }
65 
66 
67  Vector2 centroid = BBox.Centroid();
68 
69  if( theSpatialGraph.GetDrawNodeIndex() )
70  {
71  Vector2 screenCenter = MathUtil::WorldToScreen( centroid.X, centroid.Y );
72  //Print some vals
73  glColor3f(0,1.f,1.f);
74  DrawGameText( IntToString(Index), "ConsoleSmall", (int)screenCenter.X, (int)screenCenter.Y );
75  }
76 
77  if( theSpatialGraph.GetDrawGraph() && !bBlocked )
78  {
79  glColor3f(1.f,0.f,0.f);
80 
81  float linePoints[4];
82  glEnableClientState(GL_VERTEX_ARRAY);
83  glVertexPointer(2, GL_FLOAT, 0, linePoints);
84  for( unsigned int i = 0; i < Neighbors.size(); i++ )
85  {
86  if( Neighbors[i]->bBlocked || !NeighborLOS[i] )
87  continue;
88 
89  //draw centroid to centroid half way point
90  Vector2 neighbor = Neighbors[i]->BBox.Centroid();
91  neighbor = centroid + ((neighbor - centroid) * 0.6f);
92 
93  linePoints[0] = centroid.X;
94  linePoints[1] = centroid.Y;
95  linePoints[2] = neighbor.X;
96  linePoints[3] = neighbor.Y;
97  glDrawArrays(GL_LINES, 0, 2);
98  }
99  }
100 
101  if( theSpatialGraph.GetDrawGridPoints() )
102  {
103  glColor3f(1.f,0.f,0.f);
104  Vector2List gridPoints;
105  int xPoints, yPoints;
106  GetGridPoints(gridPoints, xPoints, yPoints );
107 
108  for( unsigned int i = 0; i < gridPoints.size(); i++ )
109  {
110  DrawPoint( gridPoints[i], Tree->GetSmallestDimensions().X * 0.15f );
111  }
112 
113  }
114 }
115 
116 void SpatialGraphKDNode::GetGridPoints( Vector2List& points, int& xPoints, int& yPoints )
117 {
118  xPoints = 0;
119  yPoints = 0;
120 
121  Vector2 vSmallestDimensions = Tree->GetSmallestDimensions();
122  Vector2 vMyBoxDimensions = BBox.Max - BBox.Min;
123 
124  /*
125  if( vSmallestDimensions == vMyBoxDimensions )
126  {
127  xPoints = 1;
128  yPoints = 1;
129  points.push_back( BBox.Centroid() );
130  return;
131  }
132  */
133 
134  xPoints = static_cast<int>(vMyBoxDimensions.X / vSmallestDimensions.X);
135  yPoints = static_cast<int>(vMyBoxDimensions.Y / vSmallestDimensions.Y);
136  points.reserve(xPoints*yPoints);
137 
138  Vector2 vBottomLeftStartBox( BBox.Min.X, BBox.Max.Y - vSmallestDimensions.Y );
139 
140  BoundingBox startBox( vBottomLeftStartBox, vBottomLeftStartBox + vSmallestDimensions);
141 
142  BoundingBox checkBox( startBox );
143 
144  for( int yDim = 0; yDim < yPoints; yDim++ )
145  {
146  for( int xDim = 0; xDim < xPoints; xDim++ )
147  {
148  points.push_back( checkBox.Centroid() );
149 
150  checkBox.Min.X += vSmallestDimensions.X;
151  checkBox.Max.X += vSmallestDimensions.X;
152  }
153 
154  checkBox.Min.X = startBox.Min.X;
155  checkBox.Max.X = startBox.Max.X;
156 
157  checkBox.Min.Y -= vSmallestDimensions.Y;
158  checkBox.Max.Y -= vSmallestDimensions.Y;
159  }
160 
161 }
162 
163 
164 SpatialGraph::SpatialGraph(float entityWidth, const BoundingBox& startBox )
165 {
166  _entityWidth = entityWidth;
167  float maxDimension = MathUtil::Max( startBox.Max.Y - startBox.Min.Y, startBox.Max.X - startBox.Min.X );
168  int depth = 0;
169  while( maxDimension > _entityWidth )
170  {
171  maxDimension /= 2.0f;
172  depth += 2;
173  }
174  _depth = MathUtil::Max(depth,1);
175  if( _depth > 24 )
176  _depth = 24;
177 
178  int depthMask = ~(0xFFFFFFFF << _depth);
179 
180  _dirMasks[0] = 0x1;
181  _dirMasks[1] = 0x2;
182  _dirMasks[2] = 0xaaaaaaaa & depthMask;
183  _dirMasks[3] = _dirMasks[2] >> 1;
184 
185  _root = CreateTree(_depth+1, startBox, NULL);
186 
187  //Get smallest dimension
188  _smallestDimensions = startBox.Max - startBox.Min;
189  for( int i = 0; i < _depth; i++ )
190  {
191  if( i % 2 )
192  _smallestDimensions.Y *= 0.5f;
193  else
194  _smallestDimensions.X *= 0.5f;
195  }
196 
197  ComputeNeighbors( _root );
198  ValidateNeighbors( _root );
199 }
200 
201 SpatialGraph::~SpatialGraph()
202 {
203  DeleteNode( _root );
204 }
205 
206 void SpatialGraph::DeleteNode( SpatialGraphKDNode* pNode )
207 {
208  if( pNode == NULL )
209  return;
210 
211  DeleteNode( pNode->LHC );
212  DeleteNode( pNode->RHC );
213 
214  delete pNode;
215 }
216 
217 
218 
219 SpatialGraphKDNode* SpatialGraph::FindNode(SpatialGraphKDNode* node, const BoundingBox& bbox)
220 {
221  if (node == NULL)
222  return NULL;
223  //check if this bbox fits entirely within our node
224  if (node->BBox.Contains(bbox) == Within)
225  {
226  //Check our children
227  SpatialGraphKDNode* retVal = FindNode(node->LHC, bbox );
228  if (retVal != NULL)
229  return retVal;
230  retVal = FindNode(node->RHC, bbox);
231  if( retVal != NULL )
232  return retVal;
233 
234  //otherwise, return ourselves
235  return node;
236 
237  }
238  return NULL;
239 }
240 
241 SpatialGraphKDNode* SpatialGraph::FindNode(SpatialGraphKDNode* node, const Vector2& point)
242 {
243  if (node == NULL)
244  return NULL;
245  //check if this bbox fits entirely within our node
246  if (node->BBox.Contains(point))
247  {
248  //Check our children
249  SpatialGraphKDNode* retVal = FindNode(node->LHC, point );
250  if (retVal != NULL)
251  return retVal;
252  retVal = FindNode(node->RHC, point);
253  if( retVal != NULL )
254  return retVal;
255 
256  //otherwise, return ourselves
257  return node;
258 
259  }
260  return NULL;
261 }
262 
263 
264 SpatialGraphKDNode* SpatialGraph::FindNode(const BoundingBox& bbox)
265 {
266  return FindNode(_root, bbox);
267 }
268 
269 SpatialGraphKDNode* SpatialGraph::FindNode(const Vector2& point)
270 {
271  return FindNode(_root, point);
272 }
273 
274 void SpatialGraph::Render()
275 {
276  bool bDrawAny = theSpatialGraph.GetDrawBounds() || theSpatialGraph.GetDrawGridPoints() || theSpatialGraph.GetDrawGraph() || theSpatialGraph.GetDrawBlocked();
277 
278  if( bDrawAny && _root)
279  {
280  _root->Render();
281  }
282 }
283 
284 const int __maxFixtureCount = 1024;
285 b2Fixture* __spatialGraphTempFixtures[__maxFixtureCount];
286 int __spatialGraphNumFixtures = 0;
287 
288 bool SpatialGraphManager::ReportFixture(b2Fixture* fixture)
289 {
290  if (__spatialGraphNumFixtures >= __maxFixtureCount - 1)
291  {
292  return false;
293  }
294  __spatialGraphTempFixtures[__spatialGraphNumFixtures++] = fixture;
295  return true;
296 }
297 
298 bool IsBlocked( const BoundingBox& bbox )
299 {
300 
301  b2AABB physBounds;
302  physBounds.lowerBound = b2Vec2( bbox.Min.X, bbox.Min.Y );
303  physBounds.upperBound = b2Vec2( bbox.Max.X, bbox.Max.Y );
304 
305  __spatialGraphNumFixtures = 0;
306  theWorld.GetPhysicsWorld().QueryAABB(&theSpatialGraph, physBounds);
307 
308  //No bodies here
309  if( __spatialGraphNumFixtures == 0 )
310  return false;
311 
312  b2PolygonShape shapeBoundsPoly;
313  b2Vec2 vertices[4];
314  vertices[0].Set( physBounds.lowerBound.x, physBounds.lowerBound.y );
315  vertices[1].Set( physBounds.upperBound.x, physBounds.lowerBound.y );
316  vertices[2].Set( physBounds.upperBound.x, physBounds.upperBound.y );
317  vertices[3].Set( physBounds.lowerBound.x, physBounds.upperBound.y );
318  shapeBoundsPoly.Set(vertices, 4);
319 
320  b2BodyDef fakeBodyDef;
321  //b2Vec2 center = physBounds.lowerBound + (0.5f * shapeBoundsDef.extents);
322  fakeBodyDef.position.Set(0.0f, 0.0f );
323  b2Body* fakeBody = theWorld.GetPhysicsWorld().CreateBody(&fakeBodyDef);
324  b2FixtureDef fakeFixtureDef;
325  fakeFixtureDef.shape = &shapeBoundsPoly;
326  b2Fixture* shapeBounds = fakeBody->CreateFixture(&fakeFixtureDef);
327 
328  for( int i = 0; i < __spatialGraphNumFixtures; i++ )
329  {
330  b2Fixture* pFix = __spatialGraphTempFixtures[i];
331  if( pFix->GetType() == b2Shape::e_polygon )
332  {
333  b2PolygonShape* pPolyShape = (b2PolygonShape*)pFix->GetShape();
334 
335  b2Manifold m0;
336  b2CollidePolygons(&m0, (b2PolygonShape*)shapeBounds->GetShape(), fakeBody->GetTransform(), pPolyShape, pFix->GetBody()->GetTransform());
337 
338  if( m0.pointCount > 0 )
339  {
340  theWorld.GetPhysicsWorld().DestroyBody(fakeBody);
341  return true;
342  }
343  }
344  else if( pFix->GetType() == b2Shape::e_circle )
345  {
346  b2CircleShape* pCircleShape = (b2CircleShape*)pFix->GetShape();
347  b2Manifold m0;
348  b2CollidePolygonAndCircle( &m0, (b2PolygonShape*)shapeBounds->GetShape(), fakeBody->GetTransform(), pCircleShape, pFix->GetBody()->GetTransform());
349  if( m0.pointCount > 0 )
350  {
351  theWorld.GetPhysicsWorld().DestroyBody(fakeBody);
352  return true;
353  }
354  }
355  }
356 
357  theWorld.GetPhysicsWorld().DestroyBody(fakeBody);
358 
359  return false;
360 
361 }
362 
363 SpatialGraphKDNode* SpatialGraph::CreateTree(int depth, const BoundingBox& bbox, SpatialGraphKDNode* parent, int index)
364 {
365  SpatialGraphKDNode* node = new SpatialGraphKDNode(bbox, parent);
366  node->Tree = this;
367  node->bBlocked = false;
368 
369  //query physics to see if we're blocked
370  node->bBlocked = IsBlocked( bbox );
371 
372  //Calculate my index
373  if( parent )
374  {
375  node->Index = index;
376  }
377  else
378  {
379  node->Index = 0;
380  }
381 
382  //Bail out if we reach max depth
383  depth--;
384  node->Depth = _depth - depth;
385  if (depth > 0 && node->bBlocked )
386  {
387  BoundingBox LHSbbox, RHSbbox;
388  MathUtil::SplitBoundingBox( bbox, depth % 2 ? MathUtil::AA_X : MathUtil::AA_Y, LHSbbox, RHSbbox );
389  node->LHC = CreateTree(depth, LHSbbox, node, node->Index << 1);
390  node->RHC = CreateTree(depth, RHSbbox, node, (node->Index << 1) + 1);
391 
392  int iMask = ~(0xFFFFFFFF << depth );
393  //If I have children, pad my index
394  node->Index = (node->Index << depth) | iMask;
395 
396  //If all my children are blocked, then destroy my children
397  if( IsFullyBlocked(node) )
398  {
399  DeleteNode( node->LHC );
400  node->LHC = NULL;
401  DeleteNode( node->RHC );
402  node->RHC = NULL;
403  }
404  }
405 
406  return node;
407 }
408 
409 bool SpatialGraph::IsFullyBlocked( SpatialGraphKDNode* node )
410 {
411  if( node == NULL )
412  return true;
413 
414  return node->bBlocked && IsFullyBlocked(node->LHC) && IsFullyBlocked(node->RHC);
415 }
416 
417 bool QuickContainsNode( hashmap_ns::hash_map<SpatialGraphKDNode*, int>& NodeList, SpatialGraphKDNode* pNode )
418 {
419  hashmap_ns::hash_map<SpatialGraphKDNode*, int>::iterator itr = NodeList.find( pNode );
420  return itr != NodeList.end();
421 }
422 
423 void NudgePointOnPlane( const BoundingBox& BBox, Vector2& vPointOnPlane )
424 {
425  //Get off the x planes
426  if( vPointOnPlane.X == BBox.Min.X )
427  {
428  vPointOnPlane.X += MathUtil::Epsilon;
429  }
430  else if( vPointOnPlane.X == BBox.Max.X )
431  {
432  vPointOnPlane.X -= MathUtil::Epsilon;
433  }
434 
435  //Get off the Y planes
436  if( vPointOnPlane.Y == BBox.Min.Y )
437  {
438  vPointOnPlane.Y += MathUtil::Epsilon;
439  }
440  else if( vPointOnPlane.Y == BBox.Max.Y )
441  {
442  vPointOnPlane.Y -= MathUtil::Epsilon;
443  }
444 }
445 
446 
447 bool SpatialGraph::CanGo( const Vector2& vFrom, const Vector2 vTo )
448 {
449 
450  //Get source cell
451  SpatialGraphKDNode* pSourceNode = FindNode( vFrom );
452  if( pSourceNode == NULL || pSourceNode->bBlocked)
453  return false;
454 
455  SpatialGraphKDNode* pDestNode = FindNode( vTo );
456  if( pDestNode == NULL || pDestNode->bBlocked)
457  return false;
458 
459  return CanGoInternal( vFrom, vTo, pSourceNode, pDestNode );
460 }
461 
462 bool SpatialGraph::CanGoInternal( const Vector2& vFrom, const Vector2 vTo, SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode )
463 {
464  //If source is dest, we definitely can go (we're both within bounding box)
465  if( pSourceNode == pDestNode )
466  return true;
467 
468  Vector2 vUseFrom = vFrom;
469  Vector2 vUseTo = vTo;
470 
471  NudgePointOnPlane( pSourceNode->BBox, vUseFrom );
472  NudgePointOnPlane( pDestNode->BBox, vUseTo );
473 
474  Ray2 ray = Ray2::CreateRayFromTo( vUseFrom, vUseTo );
475 
476  hashmap_ns::hash_map<SpatialGraphKDNode*, int> NodeList;
477  SpatialGraphKDNode* pCurrent = pSourceNode;
478 
479  while( true )
480  {
481  //Mark current as visited
482  NodeList[pCurrent] = 1;
483  SpatialGraphKDNode* pNearestNeighbor = NULL;
484  float fNearestNeighborDistance = MathUtil::MaxFloat;
485  //iterate over currents neighbors to see if they intersect the ray
486  for( unsigned int i = 0; i < pCurrent->Neighbors.size(); i++ )
487  {
488  SpatialGraphKDNode* pNeighbor = pCurrent->Neighbors[i];
489 
490  //Ignore neighbors we've already visited
491  if( QuickContainsNode( NodeList, pNeighbor) )
492  continue;
493 
494  float fDistanceAlongRay;
495  if( pNeighbor->BBox.Intersects( ray, fDistanceAlongRay ) )
496  {
497  if( fDistanceAlongRay < fNearestNeighborDistance )
498  {
499  fNearestNeighborDistance = fDistanceAlongRay;
500  pNearestNeighbor = pNeighbor;
501  }
502  }
503  }
504 
505  //If we couldn't find a nearest neighbor, or the neighbor is blocked bail out
506  if( pNearestNeighbor == NULL || pNearestNeighbor->bBlocked )
507  break;
508 
509  //If the nearest neighbor is our destination, we found it!
510  if( pNearestNeighbor == pDestNode )
511  return true;
512 
513  //otherwise, check our neighbor
514  pCurrent = pNearestNeighbor;
515  }
516 
517  return false;
518 }
519 
520 bool SpatialGraph::CanGoNodeToNode( SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode )
521 {
522  return CanGoInternal( pSourceNode->BBox.Centroid(), pDestNode->BBox.Centroid(), pSourceNode, pDestNode );
523 }
524 
525 
526 void SpatialGraph::AddNeighbor( SpatialGraphKDNode* node, const Vector2& pos )
527 {
528  SpatialGraphKDNode* pNeighbor = node->Tree->FindNode( pos );
529  if( pNeighbor )
530  {
531  //Add unique
532  for( unsigned int i = 0; i < node->Neighbors.size(); i++ )
533  {
534  if( node->Neighbors[i] == pNeighbor )
535  return;
536  }
537 
538  node->Neighbors.push_back( pNeighbor );
539  node->NeighborLOS.push_back(true);
540  }
541 
542 }
543 
544 int GetColumnMajorIndex( int wantX, int wantY, int maxX )
545 {
546  //Get column
547  return (wantY * maxX) + wantX;
548 }
549 
550 void SpatialGraph::ComputeNeighbors( SpatialGraphKDNode* node )
551 {
552  if( node->HasChildren() )
553  {
554  ComputeNeighbors(node->LHC);
555  ComputeNeighbors(node->RHC);
556  return;
557  }
558 
559  Vector2 checkN = Vector2::UnitY * _smallestDimensions.Y;
560  Vector2 checkS = Vector2::UnitY * -_smallestDimensions.Y;
561  Vector2 checkE = Vector2::UnitX * _smallestDimensions.X;
562  Vector2 checkW = Vector2::UnitX * -_smallestDimensions.X;
563 
564  /* Vector2 centroid = */ node->BBox.Centroid();
565 
566  Vector2List gridPoints;
567  int xPoints, yPoints;
568  node->GetGridPoints(gridPoints, xPoints, yPoints );
569 
570  //Check north neighbors
571  for( int i = 0; i < xPoints; i++ )
572  {
573  AddNeighbor( node, gridPoints[GetColumnMajorIndex(i,0,xPoints)] + checkN );
574  }
575 
576  //Check south neighbors
577  for( int i = 0; i < xPoints; i++ )
578  {
579  AddNeighbor( node, gridPoints[GetColumnMajorIndex(i,yPoints-1,xPoints)] + checkS );
580  }
581 
582  //Check east neighbors
583  for( int i = 0; i < yPoints; i++ )
584  {
585  AddNeighbor( node, gridPoints[GetColumnMajorIndex(xPoints-1,i,xPoints)] + checkE );
586  }
587 
588  //Check west neighbors
589  for( int i = 0; i < yPoints; i++ )
590  {
591  AddNeighbor( node, gridPoints[GetColumnMajorIndex(0,i,xPoints)] + checkW );
592  }
593 
594 }
595 
596 void SpatialGraph::ValidateNeighbors( SpatialGraphKDNode* node )
597 {
598  if( node->HasChildren() )
599  {
600  ValidateNeighbors(node->LHC);
601  ValidateNeighbors(node->RHC);
602  return;
603  }
604  //Validate neighbors
605  for (unsigned int i = 0; i < node->Neighbors.size(); i++ )
606  {
607  //Todo, incorporate entity width
608  if( !CanGoNodeToNode( node, node->Neighbors[i]) )
609  {
610  node->NeighborLOS[i] = false;
611  }
612  }
613 }
614 
615 
616 SpatialGraphManager* SpatialGraphManager::s_SpatialGraphManager = NULL;
617 SpatialGraphManager & SpatialGraphManager::GetInstance()
618 {
619  if( s_SpatialGraphManager == NULL)
620  {
621  s_SpatialGraphManager = new SpatialGraphManager();
622  s_SpatialGraphManager->Initialize();
623  }
624  return *s_SpatialGraphManager;
625 }
626 
627 SpatialGraphManager::SpatialGraphManager():
628 _spatialGraph(NULL),
629 _drawBounds(false),
630 _drawBlocked(false),
631 _drawGridPoints(false),
632 _drawGraph(false),
633 _drawNodeIndex(false)
634 {
635 
636 }
637 
638 SpatialGraphManager::~SpatialGraphManager()
639 {
640 
641 }
642 
643 void SpatialGraphManager::CreateGraph( float entityWidth, const BoundingBox& bounds )
644 {
645  if( _spatialGraph != NULL )
646  delete _spatialGraph;
647 
648  _spatialGraph = new SpatialGraph( entityWidth, bounds );
649 }
650 
651 //Vector2List s_tempPath;
652 void SpatialGraphManager::Render()
653 {
654  if( _spatialGraph )
655  _spatialGraph->Render();
656 }
657 
658 bool ContainsNode( SpatialGraphNeighborList& path, const SpatialGraphKDNode* node)
659 {
660  for( SpatialGraphNeighborList::iterator itr = path.begin(); itr != path.end(); itr++ )
661  {
662  if( (*itr) == node )
663  return true;
664  }
665 
666  return false;
667 }
668 
669 #include "stlastar.h"
670 
672 {
673  SpatialGraphKDNode* pNode;
674 
676  {
677  pNode = node;
678  }
679 
681  {
682  pNode = NULL;
683  }
684 
685  ~SearchInterface()
686  {
687  }
688 
689  float GoalDistanceEstimate( const SearchInterface& goal )
690  {
691  return GetCost( goal );
692  }
693 
694  bool IsGoal( const SearchInterface& goal )
695  {
696  return IsSameState(goal);
697  }
698 
699  bool IsSameState( const SearchInterface& goal )
700  {
701  return pNode == goal.pNode;
702  }
703 
704  bool GetSuccessors( AStarSearch<SearchInterface>* pSearch, SearchInterface* pParent )
705  {
706  for( unsigned int i = 0; i < pNode->Neighbors.size(); i++ )
707  {
708  SpatialGraphKDNode* successor = pNode->Neighbors[i];
709  if( (!pParent || pParent->pNode != successor) && !successor->bBlocked && pNode->NeighborLOS[i])
710  {
711  SearchInterface si(successor);
712  if( !pSearch->AddSuccessor(si) )
713  return false;
714  }
715  }
716  return true;
717  }
718 
719  float GetCost( const SearchInterface& successor )
720  {
721  return Vector2::Distance( pNode->BBox.Centroid(), successor.pNode->BBox.Centroid() );
722  }
723 };
724 
725 bool ComputeAStar( SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode, Vector2List& path )
726 {
728  SearchInterface pStart(pSourceNode);
729  SearchInterface pEnd(pDestNode);
730 
731  search.SetStartAndGoalStates( pStart, pEnd );
732 
733  while( AStarSearch<SearchInterface>::SEARCH_STATE_SEARCHING == search.SearchStep() )
734  {
735 
736  }
737 
738  int curState = search.GetState();
740  {
741  //Get path
742  for( SearchInterface* pCur = search.GetSolutionStart(); pCur != NULL; pCur = search.GetSolutionNext() )
743  {
744  path.push_back( pCur->pNode->BBox.Centroid() );
745  }
746  search.FreeSolutionNodes();
747  return true;
748  }
749 
750  return false;
751 }
752 
753 bool SpatialGraphManager::GetPath( const Vector2& source, const Vector2& dest, Vector2List& path )
754 {
755  if( _spatialGraph == NULL )
756  return false;
757 
758  //Get source cell
759  SpatialGraphKDNode* pSourceNode = _spatialGraph->FindNode( source );
760  if( pSourceNode == NULL )
761  return false;
762 
763  SpatialGraphKDNode* pDestNode = _spatialGraph->FindNode( dest );
764  if( pDestNode == NULL )
765  return false;
766 
767  path.push_back(source);
768  if( pSourceNode == pDestNode )
769  {
770  path.push_back(dest);
771  return true;
772  }
773 
774  //Compute A*
775  bool retVal = ComputeAStar(pSourceNode, pDestNode, path);
776  if( retVal == false )
777  {
778  path.clear();
779  return false;
780  }
781 
782  //otherwise, put dest on the pathlist
783  path.push_back(dest);
784 
785  return true;
786 }
787 
788 bool SpatialGraphManager::CanGo( const Vector2& from, const Vector2 to )
789 {
790 
791  if( _spatialGraph == NULL )
792  return false;
793 
794  return _spatialGraph->CanGo( from, to );
795 
796 }
797 
798 bool SpatialGraphManager::IsInPathableSpace( const Vector2& point )
799 {
800  return CanGo(point,point);
801 }
802 
803 bool SpatialGraphManager::FindNearestNonBlocked( const Vector2& fromPoint, Vector2& goTo )
804 {
805  if( _spatialGraph == NULL )
806  return false;
807 
808  SpatialGraphKDNode* pCurrentNode = _spatialGraph->FindNode( fromPoint );
809  if( pCurrentNode == NULL )
810  return false;
811 
812  float fMinDistance = MathUtil::MaxFloat;
813  SpatialGraphKDNode* pNearestNeighbor = NULL;
814  //otherwise, iterate over neighbors to find a non-blocked
815  for( unsigned int i = 0; i < pCurrentNode->Neighbors.size(); i++ )
816  {
817  SpatialGraphKDNode* pNeighbor = pCurrentNode->Neighbors[i];
818  if( pNeighbor->bBlocked )
819  continue;
820 
821  Vector2 vDir = pNeighbor->BBox.Centroid() - fromPoint;
822  Ray2 ray( fromPoint, Vector2::Normalize(vDir) );
823 
824  float distanceToBBox;
825  if( pNeighbor->BBox.Intersects(ray, distanceToBBox) )
826  {
827  if( distanceToBBox < fMinDistance )
828  {
829  fMinDistance = distanceToBBox;
830  pNearestNeighbor = pNeighbor;
831  }
832  }
833  }
834 
835  if( pNearestNeighbor != NULL )
836  {
837  goTo = pNearestNeighbor->BBox.Centroid();
838  return true;
839  }
840 
841  return false;
842 }
843 
844 
845 void SpatialGraphManager::Initialize()
846 {
847 }
848 
849 
850 void SpatialGraphManager::EnableDrawBounds(bool enable)
851 {
852  _drawBounds = enable;
853 }
854 
855 const bool SpatialGraphManager::ToggleDrawBounds()
856 {
857  EnableDrawBounds( !GetDrawBounds() );
858  return GetDrawBounds();
859 }
860 
861 const bool SpatialGraphManager::GetDrawBounds()
862 {
863  return _drawBounds;
864 }
865 
866 void SpatialGraphManager::EnableDrawBlocked(bool enable)
867 {
868  _drawBlocked = enable;
869 }
870 
871 const bool SpatialGraphManager::ToggleDrawBlocked()
872 {
873  EnableDrawBlocked( !GetDrawBlocked() );
874  return GetDrawBlocked();
875 }
876 
877 const bool SpatialGraphManager::GetDrawBlocked()
878 {
879  return _drawBlocked;
880 }
881 
882 void SpatialGraphManager::EnableDrawGridPoints(bool enable)
883 {
884  _drawGridPoints = enable;
885 }
886 
887 const bool SpatialGraphManager::ToggleDrawGridPoints()
888 {
889  EnableDrawGridPoints( !GetDrawGridPoints() );
890  return GetDrawGridPoints();
891 }
892 
893 const bool SpatialGraphManager::GetDrawGridPoints()
894 {
895  return _drawGridPoints;
896 }
897 
898 void SpatialGraphManager::EnableDrawGraph(bool enable)
899 {
900  _drawGraph = enable;
901 }
902 
903 const bool SpatialGraphManager::ToggleDrawGraph()
904 {
905  EnableDrawGraph( !GetDrawGraph() );
906  return GetDrawGraph();
907 }
908 
909 const bool SpatialGraphManager::GetDrawGraph()
910 {
911  return _drawGraph;
912 }
913 
914 void SpatialGraphManager::EnableDrawNodeIndex(bool enable)
915 {
916  _drawNodeIndex = enable;
917 }
918 
919 const bool SpatialGraphManager::ToggleDrawNodeIndex()
920 {
921  EnableDrawNodeIndex( !GetDrawNodeIndex() );
922  return GetDrawNodeIndex();
923 }
924 
925 const bool SpatialGraphManager::GetDrawNodeIndex()
926 {
927  return _drawNodeIndex;
928 }