public override void Start()
{
//Set up our obstacle course
ActorFactory.Instance.LoadLevel("maze");
//Create the bounding box that will limit the pathfinding search area
BoundingBox2D bounds = new BoundingBox2D(new Vector2(-20, -20), new Vector2(20, 20));
//Create our pathfinding graph. In our 2D worlds, this is a relatively fast
// operation -- you shouldn't be doing it every frame, but recalculating every
// so often if your world has changed is not inappropriate.
SpatialGraphManager.Instance.CreateGraph(
0.75f, //The size of the entity you want to pathfind (so the generator
// can know how small a space can be and still have it fit.)
ref bounds //The search area
);
//Create a MazeFinder (class definition below), and put him in the bottom
// left corner of the maze
_mf = new MazeFinder();
_mf.Position = new Vector2(-11.5f, -8.0f);
World.Instance.Add(_mf, 2);
//Send him to the upper right, watch him scurry
_mf.GoTo(Vector2.Zero);
// Register this object as the mouse listener
InputManager.Instance.RegisterMouseListener(this);
//Demo housekeeping below this point.
#region Demo housekeeping
String description = "This little dude is pathfinding through the area.";
description += "\n\nClick the mouse to give him a new target.";
description += "\n\nPress [B] to see the pathfinding graph.";
TextActor t = new TextActor("Console", description);
t.TextAlignment = TextActor.Alignment.Center;
t.Position = new Vector2(0.0f, -5.0f);
World.Instance.Add(t, 2);
TextActor fileLoc = new TextActor("ConsoleSmall", "DemoScreenPathfinding.cs");
fileLoc.Position = World.Instance.Camera.ScreenToWorld(5, 755);
fileLoc.Color = new Color(.3f, .3f, .3f);
World.Instance.Add(fileLoc, 2);
_objects.Add(fileLoc);
_objects.Add(t);
_objects.Add(_mf);
Actor[] walls = TagCollection.Instance.GetObjectsTagged("maze_wall");
foreach(Actor a in walls)
_objects.Add(a);
#endregion
}
public static BoundingBox2D CreateFromPoints(ref Vector2[] points)
{
if( points.Length < 1 )
return new BoundingBox2D(Vector2.Zero, Vector2.Zero);
BoundingBox2D retVal = new BoundingBox2D( new Vector2(float.MaxValue, float.MaxValue),
new Vector2(float.MinValue, float.MinValue));
for( int i = 0 ; i < points.Length; ++i )
{
retVal.Min = Vector2.Min(retVal.Min, points[i]);
retVal.Max = Vector2.Max(retVal.Max, points[i]);
}
return retVal;
}
public ContainmentType Contains(ref BoundingBox2D box)
{
if ((Max.X < box.Min.X) || (Min.X > box.Max.X))
{
return ContainmentType.Disjoint;
}
if ((Max.Y < box.Min.Y) || (Min.Y > box.Max.Y))
{
return ContainmentType.Disjoint;
}
if ((((Min.X <= box.Min.X) && (box.Max.X <= Max.X)) && ((Min.Y <= box.Min.Y) && (box.Max.Y <= Max.Y))))
{
return ContainmentType.Within;
}
return ContainmentType.Intersects;
}
public static void SplitBoundingBox(ref BoundingBox2D source, AABBSplittingAxis axis, out BoundingBox2D LHS, out BoundingBox2D RHS)
{
LHS = source;
RHS = source;
switch (axis)
{
case AABBSplittingAxis.X:
LHS.Max.X = MathHelper.Lerp(LHS.Min.X, LHS.Max.X, 0.5f);
RHS.Min.X = LHS.Max.X;
break;
case AABBSplittingAxis.Y:
LHS.Max.Y = MathHelper.Lerp(LHS.Min.Y, LHS.Max.Y, 0.5f);
RHS.Min.Y = LHS.Max.Y;
break;
}
}
public SpatialGraph(float entityWidth, ref BoundingBox2D startBox)
{
_entityWidth = entityWidth;
float maxDimension = MathHelper.Max(startBox.Max.Y - startBox.Min.Y, startBox.Max.X - startBox.Min.X);
int depth = 0;
while (maxDimension > _entityWidth)
{
maxDimension /= 2.0f;
depth += 2;
}
_depth = depth > 1 ? depth : 1;
if (_depth > 24)
_depth = 24;
uint depthMask = ~(0xFFFFFFFF << _depth);
_dirMasks[0] = 0x1;
_dirMasks[1] = 0x2;
_dirMasks[2] = 0xaaaaaaaa & depthMask;
_dirMasks[3] = _dirMasks[2] >> 1;
_root = CreateTree(_depth + 1, ref startBox, null, 0);
//Get smallest dimension
_smallestDimensions = startBox.Max - startBox.Min;
for (int i = 0; i < _depth; i++)
{
if (i % 2 != 0)
_smallestDimensions.Y *= 0.5f;
else
_smallestDimensions.X *= 0.5f;
}
ComputeNeighbors(_root);
ValidateNeighbors(_root);
}
public void CreateGraph(float entityWidth, ref BoundingBox2D bounds)
{
_spatialGraph = new SpatialGraph(entityWidth, ref bounds);
}
public void GetGridPoints(out List<Vector2> points, out int xPoints, out int yPoints)
{
xPoints = 0;
yPoints = 0;
Vector2 vSmallestDimensions = Tree.SmallestDimensions;
Vector2 vMyBoxDimensions = BBox.Max - BBox.Min;
/*
if( vSmallestDimensions == vMyBoxDimensions )
{
xPoints = 1;
yPoints = 1;
points.push_back( BBox.Centroid() );
return;
}
*/
xPoints = (int)(vMyBoxDimensions.X / vSmallestDimensions.X);
yPoints = (int)(vMyBoxDimensions.Y / vSmallestDimensions.Y);
points = new List<Vector2>(xPoints*yPoints);
Vector2 vBottomLeftStartBox = new Vector2( BBox.Min.X, BBox.Max.Y - vSmallestDimensions.Y );
BoundingBox2D startBox = new BoundingBox2D( vBottomLeftStartBox, vBottomLeftStartBox + vSmallestDimensions);
BoundingBox2D checkBox = startBox;
for( int yDim = 0; yDim < yPoints; ++yDim )
{
for( int xDim = 0; xDim < xPoints; ++xDim )
{
points.Add( checkBox.Centroid() );
checkBox.Min.X += vSmallestDimensions.X;
checkBox.Max.X += vSmallestDimensions.X;
}
checkBox.Min.X = startBox.Min.X;
checkBox.Max.X = startBox.Max.X;
checkBox.Min.Y -= vSmallestDimensions.Y;
checkBox.Max.Y -= vSmallestDimensions.Y;
}
}
public SpatialGraphKDNode(BoundingBox2D bb, SpatialGraphKDNode parent)
{
BBox = bb;
Parent = parent;
}
private SpatialGraphKDNode CreateTree(int depth, ref BoundingBox2D bbox, SpatialGraphKDNode parent, int index)
{
SpatialGraphKDNode node = new SpatialGraphKDNode(bbox, parent);
node.Tree = this;
node.Blocked = false;
//query physics to see if we're blocked
node.Blocked = IsBlocked( ref bbox );
//Calculate my index
if( parent != null )
node.Index = index;
else
node.Index = 0;
//Bail out if we reach max depth
depth--;
node.Depth = _depth - depth;
if (depth > 0 && node.Blocked )
{
BoundingBox2D LHSbbox, RHSbbox;
MathUtil.SplitBoundingBox(ref bbox, depth % 2 == 0 ? MathUtil.AABBSplittingAxis.Y : MathUtil.AABBSplittingAxis.X, out LHSbbox, out RHSbbox);
node.LHC = CreateTree(depth, ref LHSbbox, node, node.Index << 1);
node.RHC = CreateTree(depth, ref RHSbbox, node, (node.Index << 1) + 1);
uint iMask = ~(0xFFFFFFFF << depth );
//If I have children, pad my index
node.Index = (int)((((uint)node.Index) << depth) | iMask);
//If all my children are blocked, then destroy my children
if( IsFullyBlocked(node) )
{
node.LHC = null;
node.RHC = null;
}
}
return node;
}
private SpatialGraphKDNode CreateTree(int depth, ref BoundingBox2D bbox)
{
return CreateTree(depth, ref bbox, null, 0);
}
private static void NudgePointOnPlane( ref BoundingBox2D BBox, ref Vector2 vPointOnPlane )
{
//Get off the x planes
if( vPointOnPlane.X == BBox.Min.X )
{
vPointOnPlane.X += MathUtil.Epsilon;
}
else if( vPointOnPlane.X == BBox.Max.X )
{
vPointOnPlane.X -= MathUtil.Epsilon;
}
//Get off the Y planes
if( vPointOnPlane.Y == BBox.Min.Y )
{
vPointOnPlane.Y += MathUtil.Epsilon;
}
else if( vPointOnPlane.Y == BBox.Max.Y )
{
vPointOnPlane.Y -= MathUtil.Epsilon;
}
}
private static bool IsBlocked(ref BoundingBox2D bbox)
{
const int maxShapeCount = 1024;
AABB physBounds;
physBounds.LowerBound = new Vector2( bbox.Min.X, bbox.Min.Y );
physBounds.UpperBound = new Vector2( bbox.Max.X, bbox.Max.Y );
Shape[] tempShapes = new Shape[maxShapeCount];
int numBroadphase = Angel.World.Instance.PhysicsWorld.Query( physBounds, tempShapes, maxShapeCount );
//No bodies here
if( numBroadphase == 0 )
return false;
PolygonDef shapeBoundsDef = new PolygonDef();
shapeBoundsDef.VertexCount = 4;
shapeBoundsDef.Vertices[0] = new Vector2( physBounds.LowerBound.X, physBounds.LowerBound.Y );
shapeBoundsDef.Vertices[1] = new Vector2( physBounds.UpperBound.X, physBounds.LowerBound.Y );
shapeBoundsDef.Vertices[2] = new Vector2( physBounds.UpperBound.X, physBounds.UpperBound.Y );
shapeBoundsDef.Vertices[3] = new Vector2( physBounds.LowerBound.X, physBounds.UpperBound.Y );
BodyDef fakeBodyDef = new BodyDef();
//b2Vec2 center = physBounds.lowerBound + (0.5f * shapeBoundsDef.extents);
fakeBodyDef.Position = Vector2.Zero;
Body fakeBody = new Body( fakeBodyDef, Angel.World.Instance.PhysicsWorld );
PolygonShape shapeBounds = new PolygonShape( shapeBoundsDef );
for( int i = 0; i < numBroadphase; i++ )
{
Shape Sh = tempShapes[i];
if( Sh.Type == ShapeType.PolygonShape )
{
PolygonShape PolyShape = (PolygonShape)Sh;
Manifold m0 = new Manifold();
XForm xf1 = fakeBody.XForm;
XForm xf2 = PolyShape.Body.XForm;
Collision.CollidePolygons(ref m0, shapeBounds, ref xf1, PolyShape, ref xf2);
if( m0.PointCount > 0 )
return true;
}
else if( Sh.Type == ShapeType.CircleShape )
{
CircleShape CircleShape = (CircleShape)Sh;
Manifold m0 = new Manifold();
Collision.CollidePolygonAndCircle( ref m0, shapeBounds, fakeBody.XForm, CircleShape, CircleShape.Body.XForm );
if( m0.PointCount > 0 )
return true;
}
}
return false;
}
public SpatialGraphKDNode FindNode(ref BoundingBox2D bbox)
{
return FindNode(_root, ref bbox);
}
public SpatialGraphKDNode FindNode(SpatialGraphKDNode node, ref BoundingBox2D bbox)
{
if (node == null)
return null;
//check if this bbox fits entirely within our node
if (node.BBox.Contains(ref bbox) == BoundingBox2D.ContainmentType.Within)
{
//Check our children
SpatialGraphKDNode retVal = FindNode(node.LHC, ref bbox);
if (retVal != null)
return retVal;
retVal = FindNode(node.RHC, ref bbox);
if (retVal != null)
return retVal;
//otherwise, return ourselves
return node;
}
return null;
}
public static BoundingBox2D CreateMerged(ref BoundingBox2D original, ref BoundingBox2D additional)
{
return new BoundingBox2D(Vector2.Min(original.Min, additional.Min), Vector2.Max(original.Max, additional.Max));
}
