Tutorial_ObstacleIntegration.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 "common/oneworldenv.h"
#include "LabEngine/gamebot.h"
#include "LabEngine/utils/labengineutils.h"

#include "gwnavruntime/world/boxobstacle.h"
#include "gwnavruntime/world/cylinderobstacle.h"
#include "gwnavruntime/world/tagvolume.h"


//RUN_THIS_FILE


namespace
{

// RigidBodyPhysics class
//
// This class is not part of Gameware Navigation integration but is used to externalize
// rigid body physics from the obstacle integration classes.
//
class RigidBodyPhysics
{
public:
    RigidBodyPhysics()
        : m_oscillationPhase(0.0f)
        , m_oscillationVelocityFactor(1.0f)
    {}

    void Initialize(const Kaim::Vec3f& position, const Kaim::Vec3f& linearVelocity, const Kaim::Vec3f& angularVelocity = Kaim::Vec3f::Zero())
    {
        m_transform.m_translation = position;
        m_linearVelocity = linearVelocity;
        m_angularVelocity = angularVelocity;
    }

    void Update(KyFloat32 simulationStepsInSeconds)
    {
        // Fake linear oscillation
        m_oscillationPhase += simulationStepsInSeconds;
        m_oscillationVelocityFactor = cosf(m_oscillationPhase);
        const Kaim::Vec3f velocity = m_oscillationVelocityFactor * m_linearVelocity;
        m_transform.m_translation += velocity * simulationStepsInSeconds;

        // Make it rotate
        if (m_angularVelocity.IsZero() == false)
        {
            Kaim::Vec3f rotationAxis = m_angularVelocity;
            const KyFloat32 rotationSpeedRadPerSecond = rotationAxis.Normalize();
            const Kaim::Matrix3x3f rotationMatrix(rotationAxis, rotationSpeedRadPerSecond * simulationStepsInSeconds);
            m_transform.m_rotationMatrix = rotationMatrix * m_transform.m_rotationMatrix;
        }
    }

    void SetLinearVelocity(const Kaim::Vec3f& linearVelocity) { m_linearVelocity = linearVelocity; }
    void SetAngularVelocity(const Kaim::Vec3f& angularVelocity) { m_angularVelocity = angularVelocity; }

    const Kaim::Transform& GetTransform() const { return m_transform; }
    const Kaim::Vec3f& GetPosition() const { return m_transform.m_translation; }
    const Kaim::Vec3f GetLinearVelocity() const { return m_oscillationVelocityFactor * m_linearVelocity; }
    const Kaim::Vec3f& GetAngularVelocity() const { return m_angularVelocity; }

    bool IsSleeping() const { return (m_linearVelocity.IsZero() && m_angularVelocity.IsZero()); }

private:
    Kaim::Vec3f m_linearVelocity;
    Kaim::Vec3f m_angularVelocity;
    Kaim::Transform m_transform;

    // Fake oscillation
    KyFloat32 m_oscillationPhase;
    KyFloat32 m_oscillationVelocityFactor;
};


// Game object representing a cylinder-shaped obstacle.
class MyGameCylinderObstacle
{
public:
    MyGameCylinderObstacle() : m_navCylinderObstacle(KY_NULL) {}

    void Initialize(Kaim::World* world, KyFloat32 radius, KyFloat32 height, const Kaim::Vec3f& position, const Kaim::Vec3f& linearVelocity);
    void Destroy();
    void Update(KyFloat32 simulationStepsInSeconds);

    RigidBodyPhysics m_rigidBodyPhysics;
    Kaim::Ptr<Kaim::CylinderObstacle> m_navCylinderObstacle;
};

void MyGameCylinderObstacle::Initialize(Kaim::World* world, KyFloat32 radius, KyFloat32 height, const Kaim::Vec3f& position, const Kaim::Vec3f& linearVelocity)
{
    // Initialize the object that represents this obstacle in the physics system.
    m_rigidBodyPhysics.Initialize(position, linearVelocity);

    // Set up the CylinderObstacleInitConfig
    Kaim::CylinderObstacleInitConfig cylinderObstacleInitConfig;
    cylinderObstacleInitConfig.m_world = world;
    cylinderObstacleInitConfig.m_radius = radius;
    cylinderObstacleInitConfig.m_height = height;
    cylinderObstacleInitConfig.m_startPosition = m_rigidBodyPhysics.GetPosition();
    // This sets the NavTag as non-walkable: it will "cut a hole" in the NavMesh.
    cylinderObstacleInitConfig.m_navTag.SetAsExclusive();

    // Create and initialize the CylinderObstacle, and add it to its World.
    m_navCylinderObstacle = *KY_NEW Kaim::CylinderObstacle;
    m_navCylinderObstacle->Init(cylinderObstacleInitConfig);

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

void MyGameCylinderObstacle::Destroy()
{
    m_navCylinderObstacle->RemoveFromWorld();
    m_navCylinderObstacle = KY_NULL;
}

void MyGameCylinderObstacle::Update(KyFloat32 simulationStepsInSeconds)
{
    const bool isStopped = m_rigidBodyPhysics.IsSleeping();
    m_navCylinderObstacle->SetDoesTriggerTagVolume(isStopped);
    if (isStopped)
        return;

    // Update the object in the physics system.
    m_rigidBodyPhysics.Update(simulationStepsInSeconds);

    // Update the CylinderObstacle.
    m_navCylinderObstacle->SetPosition(m_rigidBodyPhysics.GetPosition());
    m_navCylinderObstacle->SetVelocity(m_rigidBodyPhysics.GetLinearVelocity());
}


// Game object representing a box-shaped obstacle
class MyGameBoxObstacle
{
public:
    MyGameBoxObstacle(): m_navBoxObstacle(KY_NULL) {}

    void Initialize(Kaim::World* world, const Kaim::Vec3f& boxHalfExtents, const Kaim::Vec3f& position, const Kaim::Vec3f& linearVelocity, const Kaim::Vec3f& angularVelocity);
    void Destroy();
    void Update(KyFloat32 simulationStepsInSeconds);

    RigidBodyPhysics m_rigidBodyPhysics;
    Kaim::Ptr<Kaim::BoxObstacle> m_navBoxObstacle;
};

void MyGameBoxObstacle::Initialize(Kaim::World* world, const Kaim::Vec3f& boxHalfExtents, const Kaim::Vec3f& position, const Kaim::Vec3f& linearVelocity, const Kaim::Vec3f& angularVelocity)
{
    // Initialize the object that represents this obstacle in the physics system.
    m_rigidBodyPhysics.Initialize(position, linearVelocity, angularVelocity);

    // Set up the BoxObstacleInitConfig.
    Kaim::BoxObstacleInitConfig boxObstacleInitConfig;
    boxObstacleInitConfig.m_world = world;
    boxObstacleInitConfig.m_localHalfExtents = boxHalfExtents;
    boxObstacleInitConfig.m_startPosition = m_rigidBodyPhysics.GetPosition();
    boxObstacleInitConfig.m_rotationMode = Kaim::BoxObstacleRotation_Yaw;
    // This sets the NavTag as non-walkable: it will "cut a hole" in the NavMesh.
    boxObstacleInitConfig.m_navTag.SetAsExclusive();

    // Create and initialize the BoxObstacle, and add it to its World
    m_navBoxObstacle = *KY_NEW Kaim::BoxObstacle;
    m_navBoxObstacle->Init(boxObstacleInitConfig);
    m_navBoxObstacle->AddToWorld();

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

void MyGameBoxObstacle::Destroy()
{
    m_navBoxObstacle->RemoveFromWorld();
    m_navBoxObstacle = KY_NULL;
}

void MyGameBoxObstacle::Update(KyFloat32 simulationStepsInSeconds)
{
    const bool isStopped = m_rigidBodyPhysics.IsSleeping();
    m_navBoxObstacle->SetDoesTriggerTagVolume(isStopped);
    if (isStopped)
        return;

    // Update the object in the physics system.
    m_rigidBodyPhysics.Update(simulationStepsInSeconds);

    // Update the BoxObstacle.
    m_navBoxObstacle->SetTransform(m_rigidBodyPhysics.GetTransform());
    m_navBoxObstacle->SetLinearVelocity(m_rigidBodyPhysics.GetLinearVelocity());
    m_navBoxObstacle->SetAngularVelocity(m_rigidBodyPhysics.GetAngularVelocity());
}


// Game object representing a dynamic customizable navigation area.
class MyGameTagVolume
{
public:
    MyGameTagVolume(): m_navTagVolume(KY_NULL) {}

    void Initialize(const Kaim::TagVolumeInitConfig& navTagVolumeInitConfig);
    void Destroy();

    Kaim::Ptr<Kaim::TagVolume> m_navTagVolume;
};

void MyGameTagVolume::Initialize(const Kaim::TagVolumeInitConfig& navTagVolumeInitConfig)
{
    // Initialize Gameware Navigation TagVolume

    m_navTagVolume = *KY_NEW Kaim::TagVolume;

    // Initialize the TagVolume
    m_navTagVolume->Init(navTagVolumeInitConfig);

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

    // We add the TagVolume to World
    m_navTagVolume->AddToWorld();
}

void MyGameTagVolume::Destroy()
{
    m_navTagVolume->RemoveFromWorld();
    m_navTagVolume = KY_NULL;
}


// Function that change from time to time some obstacle parameters
// to make this tutorial more interesting.
void UpdateScenario(KyUInt32 frameIndex, MyGameCylinderObstacle& cylinderObstacle, MyGameBoxObstacle& boxObstacle,
    MyGameTagVolume& tagVolume, Kaim::TagVolumeInitConfig& navTagVolumeInitConfig)
{
    static const KyUInt32 scenarioCycleLength = 1000;
    const KyUInt32 scenarioCycleIdx = frameIndex / scenarioCycleLength;
    const KyUInt32 scenarioFrameIdx = frameIndex % scenarioCycleLength;

    if (scenarioFrameIdx == 200)
    {
        cylinderObstacle.m_rigidBodyPhysics.SetLinearVelocity(Kaim::Vec3f::Zero());
        return;
    }

    if (scenarioFrameIdx == 300)
    {
        boxObstacle.m_rigidBodyPhysics.SetLinearVelocity(Kaim::Vec3f::Zero());
        boxObstacle.m_rigidBodyPhysics.SetAngularVelocity(Kaim::Vec3f::Zero());
        return;
    }

    if (scenarioFrameIdx == 600)
    {
        tagVolume.Destroy();
        return;
    }

    if (scenarioFrameIdx == 700)
    {
        cylinderObstacle.m_rigidBodyPhysics.SetLinearVelocity(Kaim::Vec3f(3.0f, 0.0f, 0.0f));
        return;
    }

    if (scenarioFrameIdx == 800)
    {
        const Kaim::Vec3f linearVelocity(3.0f, 0.0f, 0.0f);
        const Kaim::Vec3f angularVelocity(0.0f, 0.0f, 1.0f);
        boxObstacle.m_rigidBodyPhysics.SetLinearVelocity(linearVelocity);
        boxObstacle.m_rigidBodyPhysics.SetAngularVelocity(angularVelocity);
        return;
    }

    if (scenarioFrameIdx == 900)
    {
        if (scenarioCycleIdx % 2 == 0)
            navTagVolumeInitConfig.m_navTag.SetAsWalkable();
        else
            navTagVolumeInitConfig.m_navTag.SetAsExclusive();

        navTagVolumeInitConfig.m_navTag.m_blindDataArray[0] = scenarioCycleIdx;

        const KyFloat32 direction = 0.5f * (scenarioCycleIdx % 4) * Kaim::KY_PI;
        const Kaim::Vec2f positionOffset(cosf(direction), sinf(direction));
        const KyUInt32 pointCount = navTagVolumeInitConfig.m_points.GetCount();
        for (KyUInt32 i = 0; i < pointCount; ++i)
            navTagVolumeInitConfig.m_points[i] += positionOffset;

        tagVolume.Initialize(navTagVolumeInitConfig);
        return;
    }
}


#define TEST_ENV_CLASS OneWorldEnv
TEST_ENV {}

TUTORIAL
{
    KT_LOG_TITLE_BEGIN("TUTORIAL - Obstacle Integration");


    // Tutorial parameters

    const KyUInt32 simulationFrameCount = 60; // Increase this value if you want to visual debug this tutorial.
    const KyFloat32 simulationDeltaTimeInSeconds = 0.016f;


    // Simulation environment setup

    // Setup a world (load some NavData, start VisualDebug...)
    CHECK(env.LoadAndAddNavData("GeneratedNavData/opencastle/opencastle.NavData") != KY_NULL);
    env.StartVisualDebugUsingLab();

    // Useful references
    Kaim::World* navWorld = env.GetWorld();
    Kaim::Database* navDatabase = env.GetDefaultDatabase();

    // Create a bot with simple route
    LabEngine::GameBotInitConfig gameBotInitConfig;
    gameBotInitConfig.m_botInitConfig.m_database = navDatabase;
    gameBotInitConfig.m_botInitConfig.m_startPosition.Set(-38.7f, -9.1f,  8.7f);

    Kaim::BotConfig botConfig;
    botConfig.m_enableAvoidance = true;

    LabEngine::GameBot* gameBot = env.AddBotFromConfig(gameBotInitConfig);
    gameBot->PushWayPoint(Kaim::Vec3f(-38.7f, -9.1f,  8.7f));
    gameBot->PushWayPoint(Kaim::Vec3f(-10.8f,  7.8f, 10.9f));


    // Tutorial-specific setup

    // Create a MyGameCylinderObstacle
    const KyFloat32 cylinderRadius = 0.5f;
    const KyFloat32 cylinderHeight = 1.0f;
    const Kaim::Vec3f cylinderPosition(-29.6941f, 2.0931f, 9.53431f);
    const Kaim::Vec3f cylinderLinearVelocity(3.0f, 0.0f, 0.0f);
    MyGameCylinderObstacle myGameCylinderObstacle;
    myGameCylinderObstacle.Initialize(navWorld, cylinderRadius, cylinderHeight, cylinderPosition, cylinderLinearVelocity);

    // Create a MyGameBoxObstacle
    const Kaim::Vec3f boxHalfExtents(2.0f, 0.5f, 1.0f);
    const Kaim::Vec3f boxPosition(-24.5504f, 5.5223f, 10.3989f);
    const Kaim::Vec3f boxLinearVelocity(3.0f, 0.0f, 0.0f);
    const Kaim::Vec3f boxAngularVelocity(0.0f, 0.0f, 1.0f);
    MyGameBoxObstacle myGameBoxObstacle;
    myGameBoxObstacle.Initialize(navWorld, boxHalfExtents, boxPosition, boxLinearVelocity, boxAngularVelocity);

    // Create a MyGameTagVolume
    Kaim::TagVolumeInitConfig navTagVolumeInitConfig;
    navTagVolumeInitConfig.m_world = navWorld;
    navTagVolumeInitConfig.m_altitudeMin = 10.0f;
    navTagVolumeInitConfig.m_altitudeMax = 11.0f;
    navTagVolumeInitConfig.m_points.PushBack(Kaim::Vec2f(-19.0967f, 7.93156f));
    navTagVolumeInitConfig.m_points.PushBack(Kaim::Vec2f(-15.3928f, 4.77404f));
    navTagVolumeInitConfig.m_points.PushBack(Kaim::Vec2f(-11.9082f, 9.46633f));
    navTagVolumeInitConfig.m_points.PushBack(Kaim::Vec2f(-14.6730f, 16.2140f));
    navTagVolumeInitConfig.m_points.PushBack(Kaim::Vec2f(-14.5526f, 10.8631f));
    navTagVolumeInitConfig.m_points.PushBack(Kaim::Vec2f(-18.5613f, 10.9361f));
    navTagVolumeInitConfig.m_navTag.SetAsExclusive();
    navTagVolumeInitConfig.m_navTag.m_blindDataArray.PushBack(0);
    MyGameTagVolume myGameTagVolume;
    myGameTagVolume.Initialize(navTagVolumeInitConfig);


    // Simulation loop

    for (KyUInt32 frameIdx = 0; frameIdx < simulationFrameCount; ++frameIdx)
    {
        // Update the obstacles
        myGameCylinderObstacle.Update(simulationDeltaTimeInSeconds);
        myGameBoxObstacle.Update(simulationDeltaTimeInSeconds);

        // Update the world
        env.Update(simulationDeltaTimeInSeconds);
        LabEngine::Utils::TimedBusyWait(1000.0f * simulationDeltaTimeInSeconds);

        // Update the scenario
        UpdateScenario(frameIdx, myGameCylinderObstacle, myGameBoxObstacle, myGameTagVolume, navTagVolumeInitConfig);
    }


    // Clean up

    // Destroy the obstacle instances
    myGameCylinderObstacle.Destroy();
    myGameBoxObstacle.Destroy();
    myGameTagVolume.Destroy();

    // Clean up the world
    env.CleanUpWorld();
}

}