Tutorial_SplineFollow.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/world/navigationprofile.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"

// In this sample, we highlight most of the relevant channel spline following properties :
// - How to set it up i.e. specifying wanted Channel radius and parameters to follow the spline
// - How to add an end constraint to the spline i.e. how to specify a desired direction of arrival at the end of a given spline

//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_arrivalDirection(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, const Kaim::Vec2f& arrivalDirection);
    void Destroy();
    void UpdateLogic(KyFloat32 simulationStepsInSeconds);
    void UpdatePhysics(KyFloat32 simulationStepsInSeconds);
    bool HasArrived();

public:
    Kaim::Vec3f m_startPosition;
    Kaim::Vec3f m_destinationPosition;
    Kaim::Vec2f m_arrivalDirection; // desired arrival direction at m_destinationPosition, this direction will be used as an end constraint for the Spline

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

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

// To add a directional end constraint to the Spline to follow, a specific IPathEventListObserver must be implemented.
// This class makes PathEvents a user CheckPoint with desired direction, the CheckPoint direction will be used by the Trajectory to add an end constraint to the Spline.
class SplineEndConstraintPathEventListObserver : public Kaim::IPathEventListObserver
{
    void OnPathEventListBuildingStageDone(Kaim::Bot* bot, Kaim::PathEventList& pathEventList, KyUInt32 firstIndexOfNewEvent, FirstIntervalStatus firstIntervalStatus);
    void OnPathEventListDestroy(Kaim::Bot* /*bot*/, Kaim::PathEventList& /*pathEventList*/, DestructionPurpose /*destructionPurpose*/) {}
};

void SplineEndConstraintPathEventListObserver::OnPathEventListBuildingStageDone(Kaim::Bot* bot, Kaim::PathEventList& pathEventList, KyUInt32 firstIndexOfNewEvent, FirstIntervalStatus firstIntervalStatus)
{
    const Kaim::NavTag* previousNavTag = KY_NULL; // track previous navTag in order to put checkpoint only at event which is the NavTag entry
    for (KyUInt32 eventIndex = firstIndexOfNewEvent; eventIndex < pathEventList.GetPathEventCount(); eventIndex++)
    {
        Kaim::PathEvent& pathEvent = pathEventList.GetPathEvent(eventIndex);
        if (eventIndex >= pathEventList.GetPathEventIntervalCount()) // navTag are encoded on intervals, not on the events themselves, so last event has no interval after it
        {
            if (pathEvent.IsAtLastNodeOfPath() == true) // last event is not necessarily the last node of the path
            {
                
                MyGameEntity* gameEntity = (MyGameEntity*)(bot->GetUserData());
                pathEvent.SetCheckPointWithDirection(gameEntity->m_arrivalDirection); // set the check point status as well its direction, the direction will be use as an end constraint for the spline
            }
            break;
        }


        const Kaim::NavTag* navTag = pathEventList.GetNavTagOnIntervalAfterEvent(eventIndex);
        if (previousNavTag != KY_NULL && previousNavTag == navTag)
            continue;

        if (navTag != KY_NULL && navTag->GetWordCount() != 0)
        {
            previousNavTag = navTag;
            if (firstIntervalStatus == FirstIntervalIsExtensionOfPreviousLast && eventIndex == firstIndexOfNewEvent)
            {
                // in this case current PathEvent was treated during the previous call to OnPathEventListBuldingStageDone,
                // since FirstIntervalIsExtensionOfPreviousLast indicates firstIndexOfNewEvent is not a new PathEvent but simply extended the last PathEvent in the previous call.
                continue;

            }

            // here, we simply give the direction of the PathEdge, 
            // but you could possibly retrieve a SmartObject from the NavTag if you use it for such purpose in order to ask it for the proper direction
            // e.g. pathEvent.SetCheckPointWithDirection(GetSmartObject(navTag)->GetDesiredArrivalDirection());
            const Kaim::Path* path = pathEvent.GetPath();
            KyUInt32 edgeIdx = pathEvent.m_positionOnPath.GetEdgeIdxToMoveOnForward(); // get the edge after a node if on a node, or the current edge if on an edge
            const Kaim::Vec3f& start = path->GetPathEdgeStartPosition(edgeIdx);
            const Kaim::Vec3f& end = path->GetPathEdgeEndPosition(edgeIdx);
            Kaim::Vec2f dir = (end - start).Get2d();
            if (dir.GetSquareLength() < 0.01f*0.01f) // Typically can occur with vertical graph edge
            {
                pathEvent.SetCheckPointStatus(Kaim::CheckPointStatus_EventIsACheckPoint); // set the path event as a simple checkpoint with no direction
            }
            else
            {
                dir.Normalize();
                pathEvent.SetCheckPointWithDirection(dir); // set the check point status and its direction, the direction will be use as an end constraint for the spline
            }
        }
    }
}

// Implement a specific NavigationProfile that instantiate the custom IPathEventListObserver.
class SplineEndConstraintNavigationProfile : public Kaim::NavigationProfile < Kaim::DefaultTraverseLogic >
{
public:

    virtual Kaim::Ptr<Kaim::IPathEventListObserver> GetSharedPathEventListObserver()
    {
        if (m_splineEndConstraintPathEventListObserver == KY_NULL)
            m_splineEndConstraintPathEventListObserver = *KY_NEW SplineEndConstraintPathEventListObserver;
        return m_splineEndConstraintPathEventListObserver;
    }

public:
    static KyUInt32 s_navigationProfileId;
    Kaim::Ptr<SplineEndConstraintPathEventListObserver> m_splineEndConstraintPathEventListObserver;
};
KyUInt32 SplineEndConstraintNavigationProfile::s_navigationProfileId = KyUInt32MAXVAL;

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

    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;
    botInitConfig.m_userData = this;

    // To initialize the bot for channel pathfollowing, you first need to set the bot config
    Kaim::BotConfig botConfig;
    botConfig.m_trajectoryMode = Kaim::TrajectoryMode_Spline;

    m_navBot->Init(botInitConfig, botConfig);
    m_navBot->SetNewPathNavigationProfileId(SplineEndConstraintNavigationProfile::s_navigationProfileId);

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

    // You can then proceed with tuning the channel computer config
    Kaim::ChannelComputerConfig channelComputerConfig;
    channelComputerConfig.m_channelRadius = 5.0f;  // The clearance radius will control how wide the channel will be. Of course, it is bound by the navmesh.
    m_navBot->SetChannelComputerConfig(channelComputerConfig);

    Kaim::SplineTrajectoryConfig splineTrajectoryConfig;
    splineTrajectoryConfig.m_recomputationDistanceRatio   = 0.3f; // The spline is recomputed each time the bot have nove forward m_recomputationDistance meters...
    splineTrajectoryConfig.m_stabilityDistance            = 5.0f; // ...But we keep the spline stable for the first few m_stabilityDistance meters.
    splineTrajectoryConfig.m_maxDistanceToSplinePosition  = 0.3f; // if the bot stray too far from the spline, we will recompute it after m_maxDistanceToSplinePosition meters.
    
    // If you are sure that you entity can follow the spline i.e apply the exact output, use VelocityComputationMode_MatchSpline
    // If not, for instance if you are ADL driven, use Kaim::VelocityComputationMode_AnimationDrivenLocomotion
    splineTrajectoryConfig.m_velocityComputationMode      = Kaim::SplineTrajectoryConfig::VelocityComputationMode_MatchSpline; 
    if (splineTrajectoryConfig.m_velocityComputationMode == Kaim::SplineTrajectoryConfig::VelocityComputationMode_AnimationDrivenLocomotion)
    {
        // If you set VelocityComputationMode_AnimationDrivenLocomotion, then we use a target on spline to compute the follow velocity.
        // In that case, set that target at some distance to take your inertia into account :
        splineTrajectoryConfig.m_targetOnSplineDistance = 0.6f;
    }
    
    m_navBot->SetSplineTrajectoryConfig(splineTrajectoryConfig);
}

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

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

protected:
    Kaim::Ptr<Kaim::NavData> m_navData;

    MyGameEntity             m_entityA;
    MyGameEntity             m_entityB;
};

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_entityA.Initialize(world, 
        Kaim::Vec3f(-2.31676f,16.7353f,10.4682f), 
        Kaim::Vec3f(-3.44684f,-39.1995f,7.5668f), 
        Kaim::Vec2f(1.0f, 0.f));
    m_entityB.Initialize(world, 
        Kaim::Vec3f(-7.95491f,26.3405f,10.5491f), 
        Kaim::Vec3f(-36.3518f,61.022f,28.2375f),
        Kaim::Vec2f(-1.0f, 0.f));

    return true;
}

void MyGameLevel::UpdateLogic(float deltaTimeInSeconds)
{
    m_entityA.UpdateLogic(deltaTimeInSeconds);
    m_entityB.UpdateLogic(deltaTimeInSeconds);
}

void MyGameLevel::UpdatePhysics(float deltaTimeInSeconds)
{
    m_entityA.UpdatePhysics(deltaTimeInSeconds);
    m_entityB.UpdatePhysics(deltaTimeInSeconds);
}
void MyGameLevel::Destroy()
{
    // Destroy my entity.
    m_entityB.Destroy();
    m_entityA.Destroy();

    m_navData->RemoveFromDatabaseImmediate();
    m_navData = 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);

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

    SplineEndConstraintNavigationProfile::s_navigationProfileId = m_navWorld->AddNavigationProfile(*KY_NEW SplineEndConstraintNavigationProfile);
    if (SplineEndConstraintNavigationProfile::s_navigationProfileId == KyUInt32MAXVAL)
        return false;

// 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);
}

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;
}

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 - Channel Path Following");

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

}