Tutorial_FirstIntegration.cpp

/*
* Copyright 2016 Autodesk, Inc. All rights reserved.
* Use of this software is subject to the terms of the Autodesk license agreement and any attachments or Appendices thereto provided at the time of installation or download,
* or which otherwise accompanies this software in either electronic or hard copy form, or which is signed by you and accepted by Autodesk.
*/

#include "testfwk/Fwk.h"
#include "labengine/utils/labengineutils.h"

#include "gwnavruntime/basesystem/basesystem.h"
#include "gwnavruntime/basesystem/coordsystem.h"

#include "gwnavruntime/world/world.h"
#include "gwnavruntime/world/bot.h"

#include "gwnavruntime/visualsystem/displaylist.h"

#include "gwnavruntime/navdata/navdata.h"
#include "gwnavruntime/kernel/SF_File.h"
#include "gwnavruntime/base/fileopener.h"
#include "gwnavruntime/base/timeprofiler.h"

#include "gwnavruntime/queries/astarquery.h"
#include "gwnavruntime/queries/blobs/astarquerydisplaylistbuilder.h"

namespace
{

class MyGameEntity
{
public:

    MyGameEntity()
        : m_startPosition(0.0f, 0.0f, 0.0f)
        , m_destinationPosition(0.0f, 0.0f, 0.0f)
        , m_position(0.0f, 0.0f, 0.0f)
        , m_velocity(0.0f, 0.0f, 0.0f)
        , m_navBot(nullptr)
    {}

    void Initialize(Kaim::World* world, const Kaim::Vec3f& startPosition, const Kaim::Vec3f& destination);
    void Destroy();
    void UpdateLogic(KyFloat32 simulationStepsInSeconds);
    void UpdatePhysics(KyFloat32 simulationStepsInSeconds);
    bool HasArrived();

public:
    Kaim::Vec3f m_startPosition;
    Kaim::Vec3f m_destinationPosition;

    Kaim::Vec3f m_position;
    Kaim::Vec3f m_velocity;

    Kaim::Ptr<Kaim::Bot> m_navBot;
};

void MyGameEntity::Initialize(Kaim::World* world, const Kaim::Vec3f& startPosition, const Kaim::Vec3f& destination)
{
    m_position = startPosition;
    m_startPosition = startPosition;
    m_destinationPosition = destination;

    m_navBot = *KY_NEW Kaim::Bot;

    // We initialize the Bot at our desired start position
    Kaim::BotInitConfig botInitConfig;
    botInitConfig.m_database = world->GetDatabase(0);
    botInitConfig.m_startPosition = m_position;
    m_navBot->Init(botInitConfig);

    // We want to draw a maximum amount of information
    m_navBot->SetCurrentVisualDebugLOD(Kaim::VisualDebugLOD_Maximal);

    // We add the Bot to our Database.
    m_navBot->AddToDatabase();
}

void MyGameEntity::Destroy()
{
    if (m_navBot)
    {
        m_navBot->RemoveFromDatabase();
        m_navBot = nullptr;
    }
}

void MyGameEntity::UpdateLogic(KyFloat32 /*simulationStepsInSeconds*/)
{
    if (m_navBot->GetFollowedPath() == nullptr && m_navBot->IsComputingPath() == false) // We need to compute a Path!
    {
        // Here we ask the Bot to launch a path computation.
        // We should test the return code of this function, but as this tutorial is pretty simple, we are sure it cannot fail:
        //  - We correctly set the AStarQuery to the bot in the MyGameEntity::Initialize() function.
        //  - We do not ask for a path computation while another path computation is in process.
        // Note that Kaim::Bot::ComputeAStarAsync() will run the query during the Kaim::World::Update().
        m_navBot->ComputeAStarQueryAsync(m_destinationPosition);
    }

    // If we are arrived...
    if (HasArrived())
    {
        m_velocity = Kaim::Vec3f::Zero();

        // Clear the followed path.
        m_navBot->ClearFollowedPath();

        // Swap the positions.
        Kaim::Vec3f swap = m_destinationPosition;
        m_destinationPosition = m_startPosition;
        m_startPosition = swap;

        // Restart a move.
        m_navBot->ComputeAStarQueryAsync(m_destinationPosition);
    }
}

void MyGameEntity::UpdatePhysics(KyFloat32 simulationStepsInSeconds)
{
    // Retrieve the velocity suggested
    m_velocity = m_navBot->GetBotOutput().m_outputVelocity;

    // Perform a simple integration clamping altitude on NavMesh
    m_position = m_navBot->ComputeMoveOnNavMesh(m_velocity, simulationStepsInSeconds);

    // Inform our Bot that the entity has moved
    // Note that NavMesh spatialization will be effective after next World::Update() call
    m_navBot->SetPositionAndVelocityAndFrontDirection(m_position, simulationStepsInSeconds);
}

bool MyGameEntity::HasArrived()
{
    KyFloat32 arrivalPrecisionRadius = m_navBot->GetConfig().m_pathProgressConfig.m_checkPointRadius;
    if (m_navBot->HasReachedPosition(m_destinationPosition, arrivalPrecisionRadius))
    {
        return true;
    }

    return false;
}


class MyGameLevel
{
public:
    MyGameLevel()
        : m_navData(nullptr)
        , m_astarQuery(nullptr)
    {
    }

    bool Initialize(Kaim::World* world);
    void UpdateLogic(float deltaTimeInSeconds);
    void UpdatePhysics(float deltaTimeInSeconds);
    void Destroy();

    void TestAStarQuery(Kaim::World* world);
    void TestPath(Kaim::World* world);

protected:
    Kaim::Ptr<Kaim::NavData> m_navData;
    Kaim::Ptr<Kaim::AStarQuery<Kaim::DefaultTraverseLogic> >  m_astarQuery; // Not used for the entity, but simply to demonstrate how to run a query
    MyGameEntity             m_entity;
};

bool MyGameLevel::Initialize(Kaim::World* world)
{
    Kaim::Ptr<Kaim::File>   kaimFile;
    Kaim::DefaultFileOpener fileOpener;

    // Open the NavData file.
    const std::string navdataFilePath = NavTest::Dirs::AbsoluteInputDir + "generated/castle/castle.NavData";
    kaimFile = fileOpener.OpenFile(navdataFilePath.c_str(), Kaim::OpenMode_Read);
    if(kaimFile == nullptr)
        return false;

    // Instantiate and load the NavData from the file contents.
    m_navData = *KY_NEW Kaim::NavData;
    KyResult loadingResult = KY_ERROR;
    loadingResult = m_navData->Load(kaimFile);

    // Close the NavData file.
    kaimFile ->Close();
    kaimFile = nullptr;

    // Check that the NavData have been correctly loaded.
    if (loadingResult == KY_ERROR)
        return false;

    // Add the NavData to the Database.
    // AddToDatabaseImmediate() forces the new NavData to be added and
    // stitched immediately in the current frame. In your final game, you
    // will probably want to use AddToDatabaseAsync(), which time-slices
    // the addition of the new data over multiple frames to avoid CPU peaks.
    m_navData->Init(world->GetDatabase(0));
    m_navData->AddToDatabaseImmediate();

    // Initialize my entity
    m_entity.Initialize(world, Kaim::Vec3f(-11.5963f,1.06987f,10.4563f), Kaim::Vec3f(-16.636f,-26.7078f,8.10107f));

    // Not used for the entity, but simply to demonstrate how to run a query
    m_astarQuery = *KY_NEW Kaim::AStarQuery<Kaim::DefaultTraverseLogic>;
    m_astarQuery->BindToDatabase(world->GetDatabase(0));

    return true;
}

void MyGameLevel::TestAStarQuery(Kaim::World* world)
{
    // Choose two positions (in Navigation space) you want to test.
    // You can use the Navigation Lab to choose them.
    Kaim::Vec3f startPos = Kaim::Vec3f(-11.5963f,1.06987f,10.4563f);
    Kaim::Vec3f destPos  = Kaim::Vec3f(-16.636f,-26.7078f,8.10107f);

    // Leverage the visual debugging system.
    Kaim::DisplayList displayList(world->GetDisplayListManager());
    displayList.InitSingleFrameLifespan("Test AStar Query", "My Game");
    displayList.PushLine(startPos, startPos + Kaim::Vec3f::UnitZ(), Kaim::Color::Blue);
    displayList.PushLine(destPos,  destPos  + Kaim::Vec3f::UnitZ(), Kaim::Color::Blue);

    if(m_astarQuery->m_processStatus == Kaim::QueryNotStarted)
    {
        // Each AstarQuery must be set up to run on a specific Database.
        m_astarQuery->Initialize(startPos, destPos);

        // Blocking means that we want the result immediately.
        // Alternatively, a query can be asynchronous and time-sliced.
        m_astarQuery->PerformQueryBlocking();
    }
    else if (m_astarQuery->m_processStatus == Kaim::QueryDone)
    {
        // When the query is done, we ask for the result.
        Kaim::AStarQueryResult result = m_astarQuery->GetResult();

        // Kaim::AStarQueryResult enumerates many possible query results.
        // For now, let's focus on success.
        switch( result )
        {
        case Kaim::ASTAR_DONE_PATH_FOUND:
            {
                displayList.PushText(destPos + Kaim::Vec3f::UnitZ(), Kaim::Color(0, 255, 0), "AStar query success !");
                break;
            }
        default:
            displayList.PushLine(startPos,  destPos , Kaim::Color::Red);
            displayList.PushText(destPos + Kaim::Vec3f::UnitZ(), Kaim::Color(255, 0, 0), "AStar query failed !");
            break;
        }

        // Here, we build and send a query, so it can be displayed into the NavigationLab.
        // cf. IQuery::SendVisualDebug() for more details.
        m_astarQuery->SendVisualDebug();
    }
}

void MyGameLevel::TestPath(Kaim::World* world)
{
    Kaim::DisplayList displayList(world->GetDisplayListManager());
    displayList.InitSingleFrameLifespan("Test Path", "My Game");

    if (m_astarQuery->m_processStatus == Kaim::QueryDone)
    {
        Kaim::AStarQueryResult result = m_astarQuery->GetResult();
        if (result == Kaim::ASTAR_DONE_PATH_FOUND)
        {
            Kaim::Path* resultPath = m_astarQuery->GetPath();
            if (resultPath->GetEdgeCount() == 0)
                return;

            // Let's fake an object moving on this path
            // in a very basic way.
            static KyFloat32 ratio = 0.0f;
            ratio += 0.00005f;
            if (ratio > 1.0f)
                ratio                  = 0.0f;
            KyFloat32 positionRatio    = ratio * resultPath->GetEdgeCount();
            KyUInt32 currentEdge       = (KyUInt32)floorf(positionRatio);
            Kaim::Vec3f edgeStart      = resultPath->GetNodePosition(currentEdge);
            Kaim::Vec3f edgeEnd        = resultPath->GetNodePosition(currentEdge + 1);
            Kaim::Vec3f objectPosition = edgeStart + (positionRatio - currentEdge) * (edgeEnd - edgeStart);

            displayList.PushArrowTube(objectPosition.OffsetZ(2.0f), objectPosition, 0.05f,
                                      Kaim::ShapeColor::FillAndLine(Kaim::Color(255, 255, 0), Kaim::Color(175, 175, 0)));
        }
    }
}

void MyGameLevel::UpdateLogic(float deltaTimeInSeconds)
{
    TestAStarQuery(m_entity.m_navBot->GetWorld());
    TestPath(m_entity.m_navBot->GetWorld());
    m_entity.UpdateLogic(deltaTimeInSeconds);
}

void MyGameLevel::UpdatePhysics(float deltaTimeInSeconds)
{
    m_entity.UpdatePhysics(deltaTimeInSeconds);
}

void MyGameLevel::Destroy()
{
    // Destroy my entity.
    m_entity.Destroy();

    if (m_navData)
    {
        m_navData->RemoveFromDatabaseImmediate();
        m_navData = nullptr;
    }

    m_astarQuery = nullptr;
}

class MyGameWorld
{
public:
    MyGameWorld() : m_gameFrameIdx(0), m_navWorld(nullptr) {}
    bool Initialize(bool doVisualDebugTutorial);
    void Update(float deltaTimeInSeconds);
    void Destroy();

private:
    void UpdateLogic(float deltaTimeInSeconds);
    void UpdateNavigation(float deltaTimeInSeconds);
    void UpdatePhysics(float deltaTimeInSeconds);

    void TestVisualDebugger();

public: // internal
    KyUInt32               m_gameFrameIdx;
    Kaim::Ptr<Kaim::World> m_navWorld;
    MyGameLevel            m_gameLevel;
    Kaim::CoordSystem      m_coordSystem;
};

bool MyGameWorld::Initialize(bool doVisualDebugTutorial)
{
    // Set these values to match your engine
    KyFloat32 oneMeterInClientUnits = 1.0f;
    Kaim::CoordSystem::ClientAxis clientAxisForX = Kaim::CLIENT_X;
    Kaim::CoordSystem::ClientAxis clientAxisForY = Kaim::CLIENT_Y;
    Kaim::CoordSystem::ClientAxis clientAxisForZ = Kaim::CLIENT_Z;
    m_coordSystem.Setup(oneMeterInClientUnits, clientAxisForX, clientAxisForY, clientAxisForZ);

    const KyUInt32 databaseCount = 1;
    m_navWorld = *KY_NEW Kaim::World(databaseCount);

// Visual debugging is disabled in Release builds. Even though the symbols and functions are available, they do nothing.
// Guarding this code is not necessary, but is recommended in order to prevent useless computations.
    KY_UNUSED(doVisualDebugTutorial);
#if defined(KY_CONFIG_DEBUG_OR_DEV)
    if (doVisualDebugTutorial)
    {
        // Create a configuration object for the visual debugging server
        Kaim::VisualDebugServerConfig visualDebugServerConfig;

        // Set up its class members
        const KyUInt32 serverPort = Kaim::VisualDebugServerConfig::DefaultServerPort();
        visualDebugServerConfig.UseNavigationLab(serverPort);
        visualDebugServerConfig.SetWaitOnStart( Kaim::VisualDebugServerConfig::DoWaitOnStart );

        // Activate visual debugging
        KyResult startResult = m_navWorld->StartVisualDebug(visualDebugServerConfig);
        if (startResult == KY_ERROR)
        {
            return false;
        }
    }
#endif

    return m_gameLevel.Initialize(m_navWorld);
}

void MyGameWorld::Update(float deltaTimeInSeconds)
{
    m_navWorld->NewVisualDebugFrame(m_gameFrameIdx);
    ++m_gameFrameIdx;

    UpdateLogic(deltaTimeInSeconds);

    UpdateNavigation(deltaTimeInSeconds);

    UpdatePhysics(deltaTimeInSeconds);

    TestVisualDebugger();
}

void MyGameWorld::UpdateLogic(float deltaTimeInSeconds)      { m_gameLevel.UpdateLogic(deltaTimeInSeconds);  }
void MyGameWorld::UpdateNavigation(float deltaTimeInSeconds) { m_navWorld->Update(deltaTimeInSeconds);        }
void MyGameWorld::UpdatePhysics(float deltaTimeInSeconds)    { m_gameLevel.UpdatePhysics(deltaTimeInSeconds); }


void MyGameWorld::Destroy()
{
    m_gameLevel.Destroy();

    m_navWorld->StopVisualDebug();
    m_navWorld = nullptr;
}

void MyGameWorld::TestVisualDebugger()
{
// Visual debugging is disabled in Release builds. Even though the symbols and functions are available, they do nothing.
// Guarding this code is not necessary, but is recommended in order to prevent useless computations.
#if defined(KY_CONFIG_DEBUG_OR_DEV)
    Kaim::VisualDebugServer* visualDebugServer = m_navWorld->GetVisualDebugServer();

    // always check the state of the VisualDebugServer
    // to avoid costly construction of messages for nothing
    if (visualDebugServer == nullptr || visualDebugServer->IsReadyToSend() == false)
        return;

    const char* myCategoryName = "MyGame";

    // Sends a message to the Log window of the Navigation Lab
    static KyUInt32 framecount = 0;
    KY_LOG_MESSAGE(("FrameCount: %d", framecount));
    framecount++;

    // Sends a single data value
    KyUInt32 someKindOfInformation = 1256;
    visualDebugServer->SendUInt32("SomeKindOfInformation", someKindOfInformation, myCategoryName);

    // Sends current, average, max, and min stats computed by the game
    const char* someKindOfStat = "SomeKindOfStat";
    KyFloat32   currentValue   = 2.56f;
    KyFloat32   averageValue   = 2.56f;
    KyFloat32   maxValue       = 3.026f;
    KyFloat32   minValue       = 0.21f;
    visualDebugServer->SendStats(someKindOfStat, currentValue, averageValue, maxValue, minValue, myCategoryName);

    // Visual information is sent using a display list
    Kaim::Vec3f boxLowerCornerPosition = Kaim::Vec3f(-5.0f, -5.0f, 0.0f);
    Kaim::Vec3f boxExtents             = Kaim::Vec3f(3.0f, 2.0f, 1.0f);
    Kaim::Vec3f positionA              = Kaim::Vec3f(0.0f, 2.0f, 0.0f);
    Kaim::Vec3f positionB              = Kaim::Vec3f(10.0f, 10.0f, 0.0f);
    KyFloat32   radius                 = 0.4f;
    KyFloat32   height                 = 2.0f;

    // The Navigation coordinate system is Z-up, right-handed,
    // in meters. Always convert positions, vectors and sizes from the coordinate
    // system you use in your game. You can set up a Kaim::CoordSystem object, and use
    // its methods to do the conversions for you, as shown below.
    boxLowerCornerPosition = m_coordSystem.ClientToNavigation_Pos( boxLowerCornerPosition );
    boxExtents             = m_coordSystem.ClientToNavigation_Pos( boxExtents );
    positionA              = m_coordSystem.ClientToNavigation_Pos( positionA );
    positionB              = m_coordSystem.ClientToNavigation_Pos( positionB );
    radius                 = m_coordSystem.ClientToNavigation_Dist( radius );
    height                 = m_coordSystem.ClientToNavigation_Dist( height );

    {
        // Create a display list and send a line
        Kaim::DisplayList displayList(m_navWorld->GetDisplayListManager());
        displayList.InitSingleFrameLifespan("Simple Display List", myCategoryName);
        displayList.PushLine(positionB, positionB + Kaim::Vec3f::UnitZ(), Kaim::Color::White);
        // The DisplayList is automatically sent to the Navigation Lab
        // when it goes out of scope.
    }

    {
        // Create a second display list and send more shapes
        Kaim::DisplayList displayList(m_navWorld->GetDisplayListManager());
        displayList.InitSingleFrameLifespan("Complex Display List", myCategoryName);

        displayList.PushVerticalOctogon(positionA, height, radius, Kaim::ShapeColor::FillAndLine(Kaim::Color::Yellow));
        displayList.PushLine(positionA, positionB, Kaim::Color::Red);

        Kaim::Box3f someBox(boxLowerCornerPosition, boxLowerCornerPosition + boxExtents);
        displayList.PushBox(someBox, Kaim::ShapeColor::FillAndLine(Kaim::Color::Green));

        displayList.PushText( positionB + Kaim::Vec3f::UnitZ(), Kaim::Color(0, 255, 255), "Some text");
        // Again, the DisplayList is automatically sent to the Navigation Lab
        // when it goes out of scope.
    }
#endif
}

class MyGame
{
public:
    ~MyGame()
    {
        m_gameWorld.Destroy();
        Kaim::BaseSystem::Destroy();
    }

    bool Initialize(bool doVisualDebugTutorial)
    {
        // Create a configuration object.
        Kaim::BaseSystem::Config config;

        Kaim::BaseSystem::Init(config);

        if (Kaim::BaseSystem::IsInitialized() == false)
        {
            Kaim::BaseSystem::Destroy();
            return false;
        }

        return m_gameWorld.Initialize(doVisualDebugTutorial);
    }

    void Update(float deltaTimeInSeconds)
    {
        m_gameWorld.Update(deltaTimeInSeconds);
    }

    bool HasFinished()
    {
        if (m_gameWorld.m_navWorld->GetVisualDebugServer() != nullptr)
            return m_gameWorld.m_navWorld->GetVisualDebugServer()->IsConnectionEstablished() == false; // check if IsConnectionEstablished() and not if IsConnected() which differs by the IsSynchronizing() step (which are 1- establishing newtwork connection, 2- Synchronizing data, 3- server is fully connected)
        else
            return m_gameWorld.m_gameFrameIdx >= 60;
    }

protected:
    MyGameWorld m_gameWorld;
};


#define KT_TEST_ENV_CLASS NavTest::Env
KT_TEST_ENV {}

KT_TUTORIAL
{
    KT_LOG_TITLE_BEGIN("TUTORIAL - First Integration Test");

    MyGame myGame;
    bool doVisualDebugTutorial = false; // pass this to true in order to visual debug this tutorial from the NavigationLab
    KT_ASSERT(myGame.Initialize(doVisualDebugTutorial));

    // Game loop
    const KyFloat32 loopDurationInSecond = 1.0f / 60.0f;
    while (myGame.HasFinished() == false)
    {
        myGame.Update(loopDurationInSecond);
        LabEngine::Utils::TimedBusyWait(1000.0f * loopDurationInSecond); // fake loop duration, to have real time VisualDebug.
    }
}

}