Initial data-driven tests for pathfinder behaviour

Introduce some basic scenario-style tests for the pathfinding AI. There
are two tests:

* Test that a peep can get from a given start position to a given end
  position, and that it takes them an expected number of ticks to do so.
  Also test that they did not walk on any 'forbidden' tiles in the process,
  e.g. tiles that are completely the wrong direction from the goal etc.

* Test that a peep can *not* get from a given start position to a given
  end position after a given number of ticks.

Each test is parametric, and instantiated for multiple different
start/end positions within the provided test park. If we find a new
situation that needs a test, it should just be a matter of building
that situation in the saved game and then adding a line to the code to
set it up.

Indicating 'forbidden' tiles is done using terrain surface type IDs:
tiles that the pathfinder should never send the peep into should be
painted with the red neon surface type (index 8). This means we have
no way to forbid some path elements on a tile while allowing others,
but we don't need that right now.

Similarly, to help ensure that the test data and code are kept in
sync, the tests also require that peep start tiles are painted with
the green neon surface type (index 11) and that goal tiles are
painted with the yellow neon surface type (index 9).

test/tests/Pathfinding.cpp

@@ -0,0 +1,249 @@
+#include "TestData.h"
+#include "openrct2/core/StringReader.hpp"
+#include "openrct2/peep/Peep.h"
+#include "openrct2/scenario/Scenario.h"
+
+#include <gtest/gtest.h>
+#include <openrct2/Context.h>
+#include <openrct2/Game.h>
+#include <openrct2/OpenRCT2.h>
+#include <openrct2/ParkImporter.h>
+#include <openrct2/world/Footpath.h>
+#include <openrct2/world/Map.h>
+
+using namespace OpenRCT2;
+
+static std::ostream& operator<<(std::ostream& os, const TileCoordsXYZ& coords)
+{
+    return os << "(" << coords.x << ", " << coords.y << ", " << coords.z << ")";
+}
+
+class PathfindingTestBase : public testing::Test
+{
+public:
+    static void SetUpTestCase()
+    {
+        gOpenRCT2Headless = true;
+        gOpenRCT2NoGraphics = true;
+        _context = CreateContext();
+        const bool initialised = _context->Initialise();
+        ASSERT_TRUE(initialised);
+
+        std::string parkPath = TestData::GetParkPath("pathfinding-tests.sv6");
+        load_from_sv6(parkPath.c_str());
+        game_load_init();
+
+        bitcount_init();
+    }
+
+    void SetUp() override 
+    {
+        // Use a consistent random seed in every test
+        gScenarioSrand0 = 0x12345678;
+        gScenarioSrand1 = 0x87654321;
+    }
+
+    static void TearDownTestCase()
+    {
+        _context = nullptr;
+    }
+
+protected:
+
+    static bool FindPath(TileCoordsXYZ* pos, const TileCoordsXYZ& goal, int maxSteps)
+    {
+        // Our start position is in tile coordinates, but we need to give the peep spawn
+        // position in actual world coords (32 units per tile X/Y, 8 per Z level).
+        // Add 16 so the peep spawns in the center of the tile.
+        rct_peep* peep = peep_generate(pos->x * 32 + 16, pos->y * 32 + 16, pos->z * 8);
+
+        // Peeps that are outside of the park use specialized pathfinding which we don't want to
+        // use here
+        peep->outside_of_park = 0;
+
+        // Pick the direction the peep should initially move in, given the goal position.
+        // This will also store the goal position and initialize pathfinding data for the peep.
+        gPeepPathFindGoalPosition = goal;
+        const int32_t moveDir = peep_pathfind_choose_direction(*pos, peep);
+        if (moveDir < 0)
+        {
+            // Couldn't determine a direction to move off in
+            return false;
+        }
+
+        // We have already set up the peep's overall pathfinding goal, but we also have to set their initial
+        // 'destination' which is a close position that they will walk towards in a straight line - in this case, one
+        // tile away. Stepping the peep will move them towards their destination, and once they reach it, a new
+        // destination will be picked, to try and get the peep towards the overall pathfinding goal.
+        peep->direction = moveDir;
+        peep->destination_x = peep->x + CoordsDirectionDelta[moveDir].x;
+        peep->destination_y = peep->y + CoordsDirectionDelta[moveDir].y;
+        peep->destination_tolerance = 2;
+
+        // Repeatedly step the peep, until they reach the target position or until the expected number of steps have
+        // elapsed. Each step, check that the tile they are standing on is not marked as forbidden in the test data
+        // (red neon ground type).
+        int step = 0;
+        while (!(*pos == goal) && step < maxSteps)
+        {
+            uint8_t pathingResult = 0;
+            peep->PerformNextAction(pathingResult);
+            ++step;
+
+            pos->x = peep->x / 32;
+            pos->y = peep->y / 32;
+            pos->z = peep->z / 8;
+
+            EXPECT_PRED_FORMAT1(AssertIsNotForbiddenPosition, *pos);
+        }
+        
+        // Clean up the peep, because we're reusing this loaded context for all tests.
+        peep_sprite_remove(peep);
+
+        // Require that the number of steps taken is exactly what we expected. The pathfinder is supposed to be
+        // deterministic, and we reset the RNG seed for each test, everything should be entirely repeatable; as
+        // such a change in the number of steps taken on one of these paths needs to be reviewed.
+        EXPECT_EQ(step, maxSteps);
+
+        return *pos == goal;
+    }
+
+    static ::testing::AssertionResult AssertIsStartPosition(const char*, const TileCoordsXYZ& location)
+    {
+        return AssertPositionIsSetUp("Start", 11u, location);
+    }
+
+    static ::testing::AssertionResult AssertIsGoalPosition(const char*, const TileCoordsXYZ& location)
+    {
+        return AssertPositionIsSetUp("Goal", 9u, location);
+    }
+
+    static ::testing::AssertionResult AssertIsNotForbiddenPosition(const char*, const TileCoordsXYZ& location)
+    {
+        const uint32_t forbiddenSurfaceStyle = 8u;
+
+        const uint32_t style = GetSurfaceStyleAtLocation(TileCoordsXY(location.x, location.y));
+
+        if (style == forbiddenSurfaceStyle)
+            return ::testing::AssertionFailure()
+                << "Path traversed location " << location << ", but it is marked as a forbidden location (surface style "
+                << forbiddenSurfaceStyle << "). Either the map is set up incorrectly, or the pathfinder went the wrong way.";
+
+        return ::testing::AssertionSuccess();
+    }
+
+private:
+
+    static uint32_t GetSurfaceStyleAtLocation(const TileCoordsXY& location)
+    {
+        TileElement* element = map_get_first_element_at(location.x, location.y);
+
+        // Every tile *should* have a surface sprite, so we should be guaranteed to find
+        // something before we go off the end of the data.
+        while (element->GetType() != TILE_ELEMENT_TYPE_SURFACE)
+            element++;
+
+        return element->AsSurface()->GetSurfaceStyle();
+    }
+
+    static ::testing::AssertionResult AssertPositionIsSetUp(const char* positionKind, uint32_t expectedSurfaceStyle, const TileCoordsXYZ& location)
+    {
+        const uint32_t style = GetSurfaceStyleAtLocation(TileCoordsXY(location.x, location.y));
+
+        if (style != expectedSurfaceStyle)
+            return ::testing::AssertionFailure()
+                << positionKind << " location " << location << " should have surface style " << expectedSurfaceStyle
+                << " but actually has style " << style
+                << ". Either the test map is not set up correctly, or you got the coordinates wrong.";
+
+        return ::testing::AssertionSuccess();
+    }
+
+    static std::shared_ptr<IContext> _context;
+};
+
+std::shared_ptr<IContext> PathfindingTestBase::_context;
+
+struct SimplePathfindingScenario
+{
+    const char* name;
+    TileCoordsXYZ start;
+    TileCoordsXYZ goal;
+    uint32_t steps;
+
+    SimplePathfindingScenario(const char* _name, const TileCoordsXYZ& _start, const TileCoordsXYZ& _goal, int _steps = 10000)
+        : name(_name)
+        , start(_start)
+        , goal(_goal)
+        , steps(_steps)
+    {
+    }
+
+    friend std::ostream& operator<<(std::ostream& os, const SimplePathfindingScenario& scenario)
+    {
+        return os << scenario.start << " => " << scenario.goal;
+    }
+
+    static std::string ToName(const ::testing::TestParamInfo<SimplePathfindingScenario>& param_info)
+    {
+        return param_info.param.name;
+    }
+};
+
+class SimplePathfindingTest : public PathfindingTestBase, public ::testing::WithParamInterface<SimplePathfindingScenario>
+{
+};
+
+TEST_P(SimplePathfindingTest, CanFindPathFromStartToGoal)
+{
+    const SimplePathfindingScenario& scenario = GetParam();
+
+    ASSERT_PRED_FORMAT1(AssertIsStartPosition, scenario.start);
+    ASSERT_PRED_FORMAT1(AssertIsGoalPosition, scenario.goal);
+
+    TileCoordsXYZ pos = scenario.start;
+
+    const auto succeeded = FindPath(&pos, scenario.goal, scenario.steps) ? ::testing::AssertionSuccess()
+                                                                         : ::testing::AssertionFailure()
+            << "Failed to find path from " << scenario.start << " to " << scenario.goal << " in " << scenario.steps
+            << " steps; reached " << pos << " before giving up.";
+
+    EXPECT_TRUE(succeeded);
+}
+
+INSTANTIATE_TEST_CASE_P(
+    ForScenario, SimplePathfindingTest,
+    ::testing::Values(
+        SimplePathfindingScenario("StraightFlat", { 2, 19, 14 }, { 4, 19, 14 }, 24),
+        SimplePathfindingScenario("SBend", { 2, 17, 14 }, { 4, 16, 14 }, 39),
+        SimplePathfindingScenario("UBend", { 2, 14, 14 }, { 2, 12, 14 }, 88),
+        SimplePathfindingScenario("CBend", { 2, 10, 14 }, { 2, 7, 14 }, 133),
+        SimplePathfindingScenario("TwoEqualRoutes", { 6, 18, 14 }, { 10, 18, 14 }, 819),
+        SimplePathfindingScenario("TwoUnequalRoutes", { 6, 14, 14 }, { 10, 14, 14 }, 15643),
+        SimplePathfindingScenario("StraightUpBridge", { 2, 4, 14 }, { 4, 4, 16 }, 24),
+        SimplePathfindingScenario("StraightUpSlope", { 4, 1, 14 }, { 6, 1, 16 }, 24),
+        SimplePathfindingScenario("SelfCrossingPath", { 6, 5, 14 }, { 8, 5, 14 }, 213)),
+    SimplePathfindingScenario::ToName);
+
+class ImpossiblePathfindingTest : public PathfindingTestBase, public ::testing::WithParamInterface<SimplePathfindingScenario>
+{
+};
+
+TEST_P(ImpossiblePathfindingTest, CannotFindPathFromStartToGoal)
+{
+    const SimplePathfindingScenario& scenario = GetParam();
+    TileCoordsXYZ pos = scenario.start;
+
+    ASSERT_PRED_FORMAT1(AssertIsStartPosition, scenario.start);
+    ASSERT_PRED_FORMAT1(AssertIsGoalPosition, scenario.goal);
+
+    EXPECT_FALSE(FindPath(&pos, scenario.goal, 10000));
+}
+
+INSTANTIATE_TEST_CASE_P(
+    ForScenario, ImpossiblePathfindingTest,
+    ::testing::Values(
+        SimplePathfindingScenario("PathWithGap", { 6, 9, 14 }, { 10, 9, 14 }),
+        SimplePathfindingScenario("PathWithFences", { 6, 7, 14 }, { 10, 7, 14 }),
+        SimplePathfindingScenario("PathWithCliff", { 10, 5, 14 }, { 12, 5, 14 })),
+    SimplePathfindingScenario::ToName);

test/tests/tests.vcxproj

@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8"?>
 <Project DefaultTargets="Build" ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
   <PropertyGroup>
     <SolutionDir Condition="'$(SolutionDir)'==''">..\..\</SolutionDir>
@@ -64,6 +64,7 @@
     <ClCompile Include="Localisation.cpp" />
     <ClCompile Include="MultiLaunch.cpp" />
     <ClCompile Include="ReplayTests.cpp" />
+    <ClCompile Include="Pathfinding.cpp" />
     <ClCompile Include="RideRatings.cpp" />
     <ClCompile Include="sawyercoding_test.cpp" />
     <ClCompile Include="$(GtestDir)\src\gtest-all.cc" />