Tutorial_FirstIntegration.cpp

/*
* Copyright 2015 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 "GwNavTestFwk/GwNavTestFwk.h"
#include "GwNavTestFwk/TestEnv.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"



//RUN_THIS_FILE


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(KY_NULL)
    {}

    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()
{
    m_navBot->RemoveFromDatabase();
    m_navBot = KY_NULL;
}

void MyGameEntity::UpdateLogic(KyFloat32 /*simulationStepsInSeconds*/)
{
    if (m_navBot->GetFollowedPath() == KY_NULL && m_navBot->IsComputingNewPath() == 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::ComputeNewPathToDestination() will run the query during the Kaim::World::Update().
        m_navBot->ComputeNewPathToDestination(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->ComputeNewPathToDestination(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(KY_NULL)
        , m_astarQuery(KY_NULL)
    {       
    }

    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 = TestSystem::Instance().InputDir() + "GeneratedNavData/opencastle/opencastle.NavData";
    kaimFile = fileOpener.OpenFile(navdataFilePath.c_str(), Kaim::OpenMode_Read);
    if(kaimFile == KY_NULL)
        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 = KY_NULL;

    // Check that the NavData have been correctly loaded.
    if (KY_FAILED(loadingResult))
        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::ScopedDisplayList displayList(world, Kaim::DisplayList_Enable);
    displayList.InitSingleFrameLifespan("Test AStar Query", "My Game");
    displayList.PushLine(startPos, startPos + Kaim::Vec3f::UnitZ(), Kaim::VisualColor::Blue);
    displayList.PushLine(destPos,  destPos  + Kaim::Vec3f::UnitZ(), Kaim::VisualColor::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::VisualColor(0, 255, 0), "AStar query success !");
                break;
            }
        default:
            displayList.PushLine(startPos,  destPos , Kaim::VisualColor::Red);
            displayList.PushText(destPos + Kaim::Vec3f::UnitZ(), Kaim::VisualColor(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::ScopedDisplayList displayList(world, Kaim::DisplayList_Enable);
    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);

            Kaim::VisualShapeColor shapeColor;
            shapeColor.m_triangleColor = Kaim::VisualColor(255, 255, 0);
            shapeColor.m_lineColor     = Kaim::VisualColor(175, 175, 0);
            displayList.PushArrow(  objectPosition + Kaim::Vec3f(0.0f, 0.0f, 2.0f),
                                    objectPosition,
                                    0.05f, shapeColor);
        }
    }
}

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();

    m_navData->RemoveFromDatabaseImmediate();
    m_navData = KY_NULL;

    m_astarQuery = KY_NULL;
}

class MyGameWorld
{
public:
    MyGameWorld() : m_gameFrameIdx(0), m_navWorld(KY_NULL) {}
    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 Shipping 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);
#ifndef KY_BUILD_SHIPPING
    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 (KY_FAILED(startResult))
        {
            return false;
        }
    }
#endif

    return m_gameLevel.Initialize(m_navWorld);
}

void MyGameWorld::Update(float deltaTimeInSeconds)
{
    if (m_navWorld->GetVisualDebugServer())
        m_navWorld->GetVisualDebugServer()->NewFrame(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 = KY_NULL;
}

void MyGameWorld::TestVisualDebugger()
{
// Visual debugging is disabled in Shipping 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.
#ifndef KY_BUILD_SHIPPING 
    Kaim::VisualDebugServer* visualDebugServer = m_navWorld->GetVisualDebugServer();
    
    // always check the state of the VisualDebugServer
    // to avoid costly construction of messages for nothing
    if (visualDebugServer == KY_NULL || visualDebugServer->IsConnected() == 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 Gameware 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::ScopedDisplayList displayList(m_navWorld, Kaim::DisplayList_Enable);
        displayList.InitSingleFrameLifespan("Simple Display List", myCategoryName);
        displayList.PushLine(positionB, positionB + Kaim::Vec3f::UnitZ(), Kaim::VisualColor::White);
        // The ScopedDisplayList is automatically sent to the Navigation Lab
        // when it goes out of scope.
    }

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

        Kaim::VisualShapeColor shapeColor;
        shapeColor.m_triangleColor = Kaim::VisualColor(255, 255, 0);
        shapeColor.m_lineColor     = Kaim::VisualColor(175, 175, 0);

        displayList.PushVerticalCylinder( positionA, radius, height, 10, shapeColor);
        displayList.PushLine( positionA, positionB, Kaim::VisualColor::Red);

        shapeColor.m_triangleColor = Kaim::VisualColor(0, 255, 0, 128);
        shapeColor.m_lineColor     = Kaim::VisualColor(0, 128, 0, 255);

        Kaim::Box3f someBox(boxLowerCornerPosition, boxLowerCornerPosition + boxExtents);
        displayList.PushBox(someBox, shapeColor);

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

class MyGame
{
public:
    bool Initialize(bool doVisualDebugTutorial);
    void Update(float deltaTimeInSeconds);
    void Destroy(); 
    bool HasFinished();

protected:
    MyGameWorld m_world;
};

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

    // Copy the license key from the navigation.gamewarekey file.
    config.m_gamewareKeyConfig.m_gamewareKey = "1GAMEWARE1KEY1PROVIDED1BY1AUTODESK1";

    Kaim::BaseSystem::Init(config);

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

    return m_world.Initialize(doVisualDebugTutorial);
}

bool MyGame::HasFinished()
{
    if (m_world.m_navWorld->GetVisualDebugServer() != KY_NULL)
        return m_world.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_world.m_gameFrameIdx >= 60;
}

void MyGame::Update(float deltaTimeInSeconds)
{
    m_world.Update(deltaTimeInSeconds);
}

void MyGame::Destroy()
{
    m_world.Destroy();
    Kaim::BaseSystem::Destroy();
}


#define TEST_ENV_CLASS TestEnv
TEST_ENV {}

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
    CHECK(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.
    }

    myGame.Destroy();
}

}