Pathfinding III: Putting it All Together

Modifying the Picking and QuadTree Code

Now that we have a real map implemented, we really want our picking routine to return the MapTile that is hit by the mouse ray, rather than the world position of the QuadTreeNode’s bounding box center.  To support this, we will be adding an additional MapTile field to our QuadTreeNode structure, like so:

public class QuadTreeNode {
    public BoundingBox Bounds;
    public QuadTreeNode[] Children;
    // new property
    public MapTile MapTile { get; set; }
    // Methods...
}

Next, we need to modify the function in our Terrain class which builds the QuadTree.  Now, when our recursive function BuildQuadTree() reaches a leaf node of the QuadTree, we will need to calculate the map-space coordinate of the tile which corresponds to this leaf.  The topLeft  parameter passed into this function is in the coordinate space of our HeightMap, so we will need to divide this coordinate by the number of HeightMap values that make up one MapTile, in order to convert into map-space.  Once we have done this, we can use this coordinate to index into our map, using the GetTile() function we implemented in Part 1.

// BuildQuadTree()
if (width >= TileSize && depth >= TileSize) {
    quadNode.Children = new[] { BuildQuadTree(topLeft, new Vector2(topLeft.X + width, topLeft.Y + depth)), BuildQuadTree(new Vector2(topLeft.X + width, topLeft.Y), new Vector2(bottomRight.X, topLeft.Y + depth)), BuildQuadTree(new Vector2(topLeft.X, topLeft.Y + depth), new Vector2(topLeft.X + depth, bottomRight.Y)), BuildQuadTree(new Vector2(topLeft.X + width, topLeft.Y + depth), bottomRight) };
} else {
    // set the maptile corresponding to this leaf node of the quad tree
    var center = topLeft / TileSize ;
                
    var mapX = (int)Math.Floor(center.X);
    var mapY = (int)Math.Floor(center.Y);
    quadNode.MapTile = GetTile(mapX, mapY);


}

Now that we have tagged each leaf node of our quad tree with the MapTile that it represents, we need to modify our intersection test method to return the MapTile associated with the intersected node of the QuadTree.  We’ll return this MapTile through an additional ref parameter.

// Terrain.cs
public bool Intersect(Ray ray, ref Vector3 worldPos, ref MapTile mapPos) {
    Vector3 ret;
    QuadTreeNode ret2;
    if (!QuadTree.Intersects(ray, out ret, out ret2)) {
        return false;
    }
    ret.Y = Height(ret.X, ret.Z);
    worldPos = ret;
   // new
    mapPos = ret2.MapTile;
    return true;
}

In addition to modifying the Terrain.Intersect() method, we also need to modify the QuadTree and QuadTreeNode Intersects() methods.  These methods will return the QuadTreeNode that was hit, so that we can also have access to the BoundingBox associated with the node if necessary.  This does not materially change the actual intersection testing; we merely have a small amount of additional book-keeping to keep track of the node that was hit.

// QuadTree
public bool Intersects(Ray ray, out Vector3 hit, out QuadTreeNode node) {
    return Root.Intersects(ray, out hit, out node);
}

public bool Intersects(Ray ray, out Vector3 hit, out QuadTreeNode node) {
    hit = new Vector3(float.MaxValue);


    // This is our terminating condition
    if (Children == null) {
        float d;
        // check if the ray intersects this leaf node's bounding box
        if (!Ray.Intersects(ray, Bounds, out d)) {
            // No intersection
            node = null;
            return false;
        }
        // return the centerpoint of the leaf's bounding box
        hit = (Bounds.Minimum + Bounds.Maximum) / 2;
        node = this;
        return true;
    }

    // If the node has children, we need to intersect each child.
    // We only intersect the child's immediate bounding volume, in order to avoid fully intersecting 
    // It is possible that the closest child intersection does not actually contain the closest
    // node that intersects the ray, so we maintain a priority queue of the child nodes that were hit, 
    // indexed by the distance to intersection
    var pq = new SortedDictionary<float, QuadTreeNode>();
    foreach (var bvhNode in Children) {
        float cd;
        if (Ray.Intersects(ray, bvhNode.Bounds, out cd)) {
            while (pq.ContainsKey(cd)) {
                // perturb things slightly so that we don't have duplicate keys
                cd += MathF.Rand(-0.001f, 0.001f);
            }
            pq.Add(cd, bvhNode);
        }
    }

    // If there were no child intersections
    if (pq.Count <= 0) {
        node = null;
        return false;
    }

    // check the child intersections for the nearest intersection
    var intersect = false;
    // setup a very-far away intersection point to compare against
    var bestHit = ray.Position + ray.Direction * 1000;
    QuadTreeNode bestNode = null;
    foreach (var bvhNode in pq) {
        Vector3 thisHit;
        QuadTreeNode thisNode;
        // intersect the child node recursively
        var wasHit = bvhNode.Value.Intersects(ray, out thisHit, out thisNode);
        if (!wasHit) {
            // no intersection, continue and intersect the other children
            continue;
        }
        // Make sure that the intersection point is in front of the ray's world-space origin
        var dot = (Vector3.Dot(Vector3.Normalize(thisHit - ray.Position), ray.Direction));
        if (!(dot > 0.9f)) {
            continue;
        }

        // check that the intersection is closer than the nearest intersection found thus far
        if (!((ray.Position - thisHit).LengthSquared() < (ray.Position - bestHit).LengthSquared()))
            continue;

        // if we have found a closer intersection store the new closest intersection
        bestHit = thisHit;
        bestNode = thisNode;
        intersect = true;
    }
    // bestHit now contains the closest intersection found, or the distant sentinel value
    hit = bestHit;
    node = bestNode;
    return intersect;
}

Calculating Connected Components

Previously, we were using the CreateTileSets() method of our Terrain class to calculate which tiles in the map were connected to each other.  After some profiling, I discovered that the naïve method initially used worked, but was very inefficient when operating on maps that had many unconnected tile sets.  After a little research, I discovered that I could achieve the same effect with much less duplicated effort by tracking which tiles had been considered using a HashSet, and then performing a Depth-First search from each unvisited tile, removing tiles from the unvisited set as each was considered by the depth-first search.

private void CreateTileSets() {
    var setNo = 0;
    var unvisited = new HashSet<MapTile>();
    // scan the tiles, to create the list of walkable tiles to consider
    // assign unwalkable or unconnected tiles to unique negative tilesets
    for (var y = 0; y < _heightInTiles; y++) {
        for (var x = 0; x < _widthInTiles; x++) {
            var tile = GetTile(x, y);
            if (tile.Edges.Any(e => e != null)) {
                if (tile.Walkable) {
                    unvisited.Add(tile);
                } else {
                    tile.Set = --setNo;
                }
            } else {
                tile.Set = --setNo;
            }
        }
    }
    setNo = 0;
    // stack for depth-first search
    var stack = new Stack<MapTile>();

    while (unvisited.Any()) {
        // extract the first unvisited node in order to seed the depth-first search
        var newFirst = unvisited.First();
        stack.Push(newFirst);
        unvisited.Remove(newFirst);

        while (stack.Any()) {
            // perform the depth-first search
            var next = stack.Pop();
            next.Set = setNo;
            // Get the neighbors of this node, where the neighbor is connected to this node, 
            // and has not been visited yet
            var neighbors = next.Edges.Where(e => e != null && unvisited.Contains(e.Node2)).Select(e => e.Node2);
            foreach (var mapTile in neighbors) {
                stack.Push(mapTile);
                unvisited.Remove(mapTile);
            }
        }
        setNo++;
    }
}

A Simple Unit Class

To showcase the pathfinding code, we need an entity to move around our terrain and follow the paths generated by the pathfinding routine.  To simplify rendering and updating this entity, we’ll create a very simple Unit class which will encapsulate the 3D model that we will render, along with some other information to track the entity’s position and progress along the path.  Our Unit class looks like this:

public class Unit : DisposableClass {
    // offset from the terrain surface to render the model
    private const float HeightOffset = 0.1f;
    private bool _disposed;

    // 3D model instance for this entity
    private readonly BasicModelInstance _modelInstance;

    // current MapTile this entity is occupying
    private MapTile MapTile { get; set; }

    // current path the entity is following
    private List<MapTile> _path = new List<MapTile>();
    // world-positions of the MapTiles the entity is traveling between
    private Vector3 _lastWP, _nextWP;
    // index of the current node in the path the entity is following
    private int _activeWP;
        
    // world-space position of the entity
    private Vector3 _position;

    private readonly Terrain _terrain;

    // movement related properties
    private bool Moving { get; set; }
    private float MovePrc { get; set; }
    private float Time { get; set; }
    private float Speed { get; set; }
}

To create a Unit instance, we need to pass in a ModelInstance, the starting MapTile for the unit, and a reference to the Terrain object that the Unit will be navigating.

public Unit( BasicModelInstance model, MapTile mp, Terrain terrain ) {
    _modelInstance = model;
    MapTile = mp;
    _terrain = terrain;
    _position = mp.WorldPos;
    _position.Y += HeightOffset;
    Time = 0.0f;
    _activeWP = 0;
    Moving = false;
    MovePrc = 0;

    Speed = 1.0f;
}

// Usage:
_sphereModel = new BasicModel();
_sphereModel.CreateSphere(Device, 0.25f, 10, 10);
_sphereModel.Materials[0] = new Material {
    Ambient = new Color4(63, 0,0),
    Diffuse = Color.Red,
    Specular = new Color4(32, 1.0f, 1.0f, 1.0f)
};
_sphereModel.DiffuseMapSRV[0] = _whiteTex;

_sphere = new BasicModelInstance (_sphereModel );
            
_unit = new Unit(_sphere, _terrain.GetTile(511,511), _terrain);

To update the Unit, we will create an Update() method.  This method should be called as part of our application’s UpdateScene() method.  We check to see if the unit is currently moving between nodes on the map, and increment the MovePrc member according to the Unit’s speed and the time since the last frame (dt).  If the update MovePrc is greater than or equal to 1, we check to see if the Unit has any further nodes to traverse in its current path.  If it does, we start the unit moving towards the next waypoint using the MoveUnit() method.  Next, we interpolate the current position of the unit between its last waypoint and the current waypoint according to MovePrc.  Finally, we update the World matrix of the Unit’s ModelInstance, so that the unit will be rendered in the correct location.

public void Update(float dt) {
    Time += dt * Speed;

    if (Moving) {
        if (MovePrc < 1.0f) {
            MovePrc += dt * Speed;
        }
        if (MovePrc > 1.0f) {
            MovePrc = 1.0f;
        }
        if (Math.Abs(MovePrc - 1.0f) < float.Epsilon) {
            if (_activeWP + 1 >= _path.Count) {
                // done following path
                Moving = false;
            } else {
                // move to the next leg of the path
                _activeWP++;
                MoveUnit(_path[_activeWP]);
            }
        }
        // move the unit towards the next waypoint on the path
        _position = Vector3.Lerp(_lastWP, _nextWP, MovePrc);
    }
    // set the world position of the model for rendering
    _modelInstance.World = Matrix.Translation(_position);
}

The MoveUnit method updates the Unit’s waypoint coordinates, its current position, and resets the MovePrc counter.

private void MoveUnit(MapTile to) {
    // set the unit's last position to its current position
    _lastWP = MapTile.WorldPos;
    _lastWP.Y = _terrain.Height(_lastWP.X, _lastWP.Z) + HeightOffset;
    // set the unit's position to the next leg in the path
    MapTile = to;
    MovePrc = 0.0f;
    // set the next position to the next leg's position
    _nextWP = MapTile.WorldPos;
    _nextWP.Y = _terrain.Height(_nextWP.X, _nextWP.Z) + HeightOffset;
}

To order the unit to move to a new goal location, we will provide the Goto() method.  This method clears the Unit’s current path, and then uses the Terrain.GetPath() function to calculate the path to the new goal location.

public void Goto(MapTile mp) {
    if (_terrain == null) return;

    _path.Clear();
    _activeWP = 0;

    if (Moving) {
        _path.Add(MapTile);
        var tmpPath = _terrain.GetPath(MapTile.MapPosition, mp.MapPosition);
        _path.AddRange(tmpPath);
    } else {
        _path = _terrain.GetPath(MapTile.MapPosition, mp.MapPosition);
        if (_path.Count <= 0) {
            // unit is already at goal position
            return;
        }
        Moving = true;
        MoveUnit(_path[_activeWP]);
    }
}

Finally, we will provide a simple Render() method to draw the unit, using it’s ModelInstance to do the heavy lifting.

public void Render(DeviceContext dc, EffectPass effectPass, Matrix view, Matrix proj) {
    _modelInstance.Draw(dc, effectPass, view, proj, RenderMode.Basic);
}

With this unit class in place, we can modify the click handler in our application to order our unit around the terrain using right-clicks, like so:

// PathfindingDemo.cs
protected override void OnMouseDown(object sender, MouseEventArgs e) {
    switch (e.Button) {
        case MouseButtons.Left:
            _minimap.OnClick(e);
            _lastMousePos = e.Location;
            Window.Capture = true;
            break;
        case MouseButtons.Right:
            // move the unit around using the right clicks
            var ray = _camera.GetPickingRay(new Vector2(e.X, e.Y), new Vector2(Viewport.Width, Viewport.Height));

            var tile = new MapTile();
            var worldPos = new Vector3();

            // do intersection test
            if (!_terrain.Intersect(ray, ref worldPos, ref tile)) {
                return;
            }
            Console.WriteLine("Clicked at " + worldPos.ToString());
            if (tile == null) {
                return;
            }
            // move the unit towards the new goal
            Console.WriteLine("Hit tile " + tile.MapPosition);
            Console.WriteLine("Moving unit to " + tile.MapPosition);
            _unit.Goto(tile);

            break;
    }
}

Rendering Unwalkable Terrain

To make it a little clearer to see the tiles in our terrain that are impassible, we will make some changes to our renderer to generate a texture map to represent the unwalkable areas of the map, and modify our terrain shader to draw these areas with a special texture.  We will encapsulate this functionality into a new nested class in our TerrainRenderer class, WalkMap.  WalkMap consists of two ShaderResourceViews, WalkableTiles, which contains the map indicating the areas which are walkable/unwalkable, and UnwalkableSRV, which contains the special texture to render where the map is unwalkable.

// TerrainRenderer.cs
private class WalkMap : DisposableClass {
    private bool _disposed;
    private readonly TerrainRenderer _terrainRenderer;
    internal ShaderResourceView WalkableTiles;
    internal ShaderResourceView UnwalkableSRV;
}

Creating the WalkMap is relatively simple.  We will create the WalkMap object for the TerrainRenderer as the final step in our Init() method.  We will load the special unwalkable texture from file, and then calculate the map texture which determines whether a tile in the terrain should be rendered using the texture or not.

// TerrainRenderer.cs
public void Init(Device device, DeviceContext dc, Terrain terrain) {
    // other initialization...
    D3DApp.GD3DApp.ProgressUpdate.Draw(1.0f, "Terrain initialized");
    _walkMap = new WalkMap(this);
}

// WalkMap
public WalkMap(TerrainRenderer terrainRenderer) {
    _terrainRenderer = terrainRenderer;
    UnwalkableSRV = ShaderResourceView.FromFile(terrainRenderer._device, "textures/unwalkable.png");
    CreateWalkableTexture(
        terrainRenderer._terrain.Tiles, 
        terrainRenderer._terrain.WidthInTiles, 
        terrainRenderer._terrain.HeightInTiles
    );
}

The walkable map is a texture with the same dimensions as our tile map.  We will use a single-channel red pixel format, in order to save some memory.  Walkable tiles will be represented with black pixels, while unwalkable tiles will be white.  In order to smooth out the transitions from the normal, walkable texture and the new unwalkable texture, we will do a single pass of bilinear filtering on the map texture.

private void CreateWalkableTexture(IList<MapTile> tiles, int widthInTiles, int heightInTiles) {
    // create the texture description for the walkable map
    // it should have the same dimensions as the tile map
    var desc = new Texture2DDescription {
        ArraySize = 1,
        BindFlags = BindFlags.ShaderResource,
        CpuAccessFlags = CpuAccessFlags.None,
        Format = Format.R8_UNorm,
        Height = heightInTiles,
        Width = widthInTiles,
        MipLevels = 1,
        OptionFlags = ResourceOptionFlags.None,
        SampleDescription = new SampleDescription(1, 0),
        Usage = ResourceUsage.Default
    };

    // create the pixel data
    var colors = new List<byte>();
    for (var y = 0; y < heightInTiles; y++) {
        for (var x = 0; x < widthInTiles; x++) {
            // walkable tiles are black, unwalkable tiles are white
            colors.Add((byte)(tiles[x + widthInTiles * y].Walkable ? 0 : 255));
        }
    }
    // do a bilinear smoothing on the walkable map, to smooth the transition between the normal and unwalkable textures
    for (var y = 0; y < heightInTiles; y++) {
        for (var x = 0; x < widthInTiles; x++) {
            float temp = 0;
            var num = 0;
            for (var y1 = y - 1; y1 <= y + 1; y1++) {
                for (var x1 = x - 1; x1 <= x + 1; x1++) {
                    if (y1 < 0 || y1 >= heightInTiles || x1 < 0 || x1 >= widthInTiles) {
                        continue;
                    }
                    temp += colors[x1 + y1 * widthInTiles];
                    num++;
                }
            }
            colors[x + y * widthInTiles] = (byte)(temp / num);
        }
    }

    // create the texture from the pixel data and create the ShaderResourceView
    var walkMap = new Texture2D(_terrainRenderer._device, desc, new DataRectangle(widthInTiles * sizeof(byte), new DataStream(colors.ToArray(), false, false)));
    WalkableTiles = new ShaderResourceView(_terrainRenderer._device, walkMap);

    Util.ReleaseCom(ref walkMap);
}

Lastly, since this class is managing two DirectX ShaderResourceViews, we will subclass WalkMap from our DisposableClass, and provide a Dispose method to release these resources.

protected override void Dispose(bool disposing) {
    if (!_disposed) {
        if (disposing) {
            Util.ReleaseCom(ref WalkableTiles);
            Util.ReleaseCom(ref UnwalkableSRV);
        }
        _disposed = true;
    }
    base.Dispose(disposing);
}

Rendering the WalkMap

To render the WalkMap, we will need to set two additional shader textures in our TerrainRenderer.Render() method, corresponding to the two WalkMap ShaderResourceViews.  I’ll leave it up to the reader to determine the necessary changes to the TerrainEffect shader wrapper, since it’s no different than any of the other shader textures we have used thus far.

Effects.TerrainFX.SetWalkMap(_walkMap.WalkableTiles);
Effects.TerrainFX.SetUnwalkableTex(_walkMap.UnwalkableSRV);

In our Terrain.fx shader file, we will need to add two additional Texture2D references to contain the WalkMap textures, like so:

Texture2D gWalkMap;
Texture2D gUnwalkable;

Next, we will need to make some changes in the pixel shader.  After we calculate the SSAO ambient modifier, we need to sample the walkable map, to determine how walkable the pixel that we are drawing is.  Here, we will use a linear sampler, using the unscaled texture coordinate, in the same fashion that we sample the normal texture blendmap.  We will also sample the special unwalkable texture, this time using the scaled texture coordinate, as we do when sampling the normal terrain textures.  Then, we linearly interpolate between the normal terrain texture color value and the unwalkable texture, according to the value sampled from the walkable map; thus, black pixels in the walkable map will use just the normal texture, white pixels will use only the unwalkable texture, and we’ll get a blend of the two on the walkable/unwalkable transistions.  Then, we do our normal lighting calculations on the resulting texture color.

// PS in Terrain.fx
// new for ssao
pin.SsaoPosH /= pin.SsaoPosH.w;
float ambientAccess = gSsaoMap.SampleLevel(samLinear, pin.SsaoPosH.xy, 0.0f).r;

// new for walkable/unwalkable
float walkFactor = gWalkMap.SampleLevel(samLinear, pin.Tex, 0);
float4 unwalkable = gUnwalkable.Sample(samLinear, pin.TiledTex);

texColor = lerp(texColor, unwalkable, walkFactor);
float4 litColor = texColor;
// lighting calculations
if (gLightCount > 0) {
// ...

With this change implemented, we can easily see the areas of the terrain that are impassible, as you can see in the screen shot below:

Next Time

I think that I am going to give terrain rendering a rest for a while, at least until I have a better idea of where I’m going with this project.  Next up, I’m going to start implementing a physics engine, working from Game Physics Engine Development by Ian Millington .  I’m hoping that this will lead to some more bite-sized content, so I can get back to a little bit more regular posting schedule.  Life has been pretty crazy the last couple of months, and that makes it hard to crank through creating the code and then writing up these more in-depth posts…