Tutorial_NavMeshTraversal.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 "common/oneworldenv.h"

#include "LabEngine/gamebot.h"
#include "LabEngine/base/kaimlogimplementation.h"
#include "LabEngine/base/kaimfileopener.h"
#include "LabEngine/utils/labengineutils.h"

#include "gwnavruntime/channel/channelarraycomputer.h"
#include "gwnavruntime/navmesh/navmeshtypes.h"
#include "gwnavruntime/world/world.h"
#include "gwnavruntime/world/bot.h"
#include "gwnavruntime/navdata/navdata.h"
#include "LabEngine/randompoints/randompointsmanager.h"

// This tutorial covers use of NavMesh traversals.

//RUN_THIS_FILE

namespace
{

// BreadthFirstSearchVisitor which validates points form a Grid against the NavMesh
template<class TraverseLogic>
class PointGridValidator
{
public:
    void Initialize(const Kaim::DatabaseGenMetrics& genMetrics, const Kaim::Vec3f& center, KyFloat32 halfWidth, KyFloat32 distanceBetweenPoints, void* traverseLogicUserData)
    {
        m_origin = center - Kaim::Vec3f(halfWidth, halfWidth, halfWidth);
        m_width = 2.f * halfWidth;
        m_pointDistance = distanceBetweenPoints;
        m_traverseLogicUserData = traverseLogicUserData;

        // compute CellBox of the grid
        const Kaim::CellPos cellPos = genMetrics.ComputeCellPos(m_origin);
        const KyInt32 widthCellSize = (KyInt32)(genMetrics.GetNearestInteger64FromFloatValue(m_width) / genMetrics.m_cellSizeInCoord) + 1;
        const Kaim::CellPos minCellPos(cellPos.x, cellPos.y);
        const Kaim::CellPos maxCellPos(cellPos.x + widthCellSize, cellPos.y + widthCellSize);
        m_cellBox.Set(minCellPos, maxCellPos);

        // reserve grid points
        KyInt32 pointCountOnXOrYAxis = 1 + KyInt32(m_width / m_pointDistance);
        KyInt32 pointCountOnZAxis = 1 + KyInt32(m_width / genMetrics.GetGenerationHeight());
        m_validGridPoints.Clear();
        m_validGridPoints.Reserve(pointCountOnXOrYAxis * pointCountOnXOrYAxis * pointCountOnZAxis);
    }

    Kaim::CellBox GetCellBox() { return m_cellBox; }


    Kaim::KyArray<Kaim::Vec3f>& GetValidPoints() { return m_validGridPoints; }

private:
    void RetrieveNeighborTriangle(const Kaim::NavTriangleRawPtr& triangleRawPtr, KyUInt32 halfEdgeIdx)
    {
        const Kaim::NavHalfEdgeIdx firstNavHalfEdgeIdx = Kaim::NavFloorBlob::NavTriangleIdxToFirstNavHalfEdgeIdx(triangleRawPtr.GetTriangleIdx());
        const Kaim::NavFloorRawPtr& navFloorRawPtr = triangleRawPtr.m_navFloorRawPtr;
        Kaim::NavHalfEdgeRawPtr navhalfEdgeRawPtr(navFloorRawPtr, firstNavHalfEdgeIdx+halfEdgeIdx);

        Kaim::NavHalfEdgeRawPtr neighborNavHalfEdgeRawPtr;
        if (navhalfEdgeRawPtr.IsHalfEdgeCrossable<TraverseLogic>(m_traverseLogicUserData, neighborNavHalfEdgeRawPtr) == true)
        {
            // edge is traversable, register it as neighbor
            m_neighborTrianglePtr[halfEdgeIdx].Set(
                neighborNavHalfEdgeRawPtr.GetNavFloor(),
                Kaim::NavFloorBlob::NavHalfEdgeIdxToTriangleIdx(neighborNavHalfEdgeRawPtr.GetHalfEdgeIdx())
                );
        }
        else
        {
            m_neighborTrianglePtr[halfEdgeIdx].Invalidate();
        }       
    }

public: // internal

    // --- Implicit BreadthFirstSearchTraversal visitor interface ---

    void Visit(const Kaim::NavTriangleRawPtr& triangleRawPtr, const Kaim::TriangleStatusInGrid& /*triangleStatus*/)
    {
        RetrieveNeighborTriangle(triangleRawPtr, 0);
        RetrieveNeighborTriangle(triangleRawPtr, 1);
        RetrieveNeighborTriangle(triangleRawPtr, 2);

        Kaim::Triangle3f triangle;
        triangleRawPtr.GetVerticesPos3f(triangle);
        KyFloat32 minX = Kaim::Max(m_origin.x, Kaim::Min(triangle[0].x, Kaim::Min(triangle[1].x, triangle[2].x)));
        KyFloat32 maxX = Kaim::Min(m_origin.x + m_width, Kaim::Max(triangle[0].x, Kaim::Max(triangle[1].x, triangle[2].x)));
        KyInt32 minXInGrid = KyInt32((minX - m_origin.x) / m_pointDistance);
        KyInt32 maxXInGrid = 1 + KyInt32((maxX - m_origin.x) / m_pointDistance);

        KyFloat32 minY = Kaim::Max(m_origin.y, Kaim::Min(triangle[0].y, Kaim::Min(triangle[1].y, triangle[2].y)));
        KyFloat32 maxY = Kaim::Min(m_origin.y + m_width, Kaim::Max(triangle[0].y, Kaim::Max(triangle[1].y, triangle[2].y)));
        KyInt32 minYInGrid = KyInt32((minY - m_origin.y) / m_pointDistance);
        KyInt32 maxYInGrid = 1 + KyInt32((maxY - m_origin.y) / m_pointDistance);

        for (KyInt32 x = minXInGrid; x < maxXInGrid; ++x)
        {
            for (KyInt32 y = minYInGrid; y < maxYInGrid; ++y)
            {
                Kaim::Vec3f pointInGrid(m_origin);
                pointInGrid.x += x * m_pointDistance;
                pointInGrid.y += y * m_pointDistance;
                if (Kaim::Intersections::IsPointInsideTriangle2d(pointInGrid, triangle[0], triangle[1], triangle[2]))
                {
                    pointInGrid.z = Kaim::Intersections::ComputeAltitudeOfPointInTriangle(pointInGrid, triangle[0], triangle[1], triangle[2]);
                    m_validGridPoints.PushBack(pointInGrid);
                }
            }
        }
    }

    bool IsSearchFinished()
    {
        return false; //let the search end when there is no more triangle to visit (cf. ShouldVisitTriangle)
    }

    bool ShouldVisitTriangle(const Kaim::NavTriangleRawPtr& triangleRawPtr)
    {
        return m_cellBox.IsInside(triangleRawPtr.GetCellPos()) 
            && triangleRawPtr.GetNavFloorBlob()->m_altitudeRange.DoesIntersect(m_origin.z, m_origin.z + m_width);
    }

    bool ShouldVisitNeighborTriangle(const Kaim::NavTriangleRawPtr& /*triangleRawPtr*/, KyUInt32 indexOfNeighborTriangle)
    {
        return m_neighborTrianglePtr[indexOfNeighborTriangle].IsValid() &&
            ShouldVisitTriangle(m_neighborTrianglePtr[indexOfNeighborTriangle]);
    }

    Kaim::NavTriangleRawPtr GetNeighborTriangle(const Kaim::NavTriangleRawPtr& /*triangleRawPtr*/, KyUInt32 indexOfNeighborTriangle)
    {
        return m_neighborTrianglePtr[indexOfNeighborTriangle];
    }

private:
    Kaim::Vec3f m_origin;
    KyFloat32 m_width;
    KyFloat32 m_pointDistance;
    void* m_traverseLogicUserData;
        
    Kaim::CellBox m_cellBox;    
    Kaim::NavTriangleRawPtr m_neighborTrianglePtr[3];

    Kaim::KyArray<Kaim::Vec3f> m_validGridPoints;
};


#define TEST_ENV_CLASS OneWorldEnv
TEST_ENV {}


TUTORIAL // 
{
    KT_LOG_TITLE_BEGIN("TUTORIAL - BreadthFirstSearchTraversal");

    // Setup a world (load some NavData, start VisualDebug...)
    CHECK(env.LoadAndAddNavData("GeneratedNavData/opencastle/opencastle.NavData") != KY_NULL);
    env.StartVisualDebugUsingLocalFile("BreadthFirstSearchTraversal.VisualDebug"); // Load this file in the NavigationLab to view the result of this tutorial.
    env.Update();
    env.Update();

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

    // Create a bot with no route
    LabEngine::GameBotInitConfig gameBotInitConfig;
    gameBotInitConfig.m_botInitConfig.m_database = navDatabase;
    gameBotInitConfig.m_botInitConfig.m_startPosition.Set(-3.2222f,36.358f,10.486f);
    Kaim::Ptr<LabEngine::GameBot> gameBot = env.AddBotFromConfig(gameBotInitConfig);
    env.Update();

    KyUInt32 displayListIdentifier = KyUInt32MAXVAL;
    LabEngine::ReproducibleRand reproductibleRand;
    PointGridValidator<LabEngine::GameTraverseLogic> pointGridValidator;
    for (KyUInt32 loopIdx = 0; loopIdx < 5 ; ++loopIdx)
    {
        // Initialize the visitor
        const Kaim::DatabaseGenMetrics& genMetrics = navDatabase->GetDatabaseGenMetrics();
        const KyFloat32 distanceAroundPosition = 10.f * (1+loopIdx);
        const KyFloat32 distanceBetweenPoints = genMetrics.GetGenerationRadius() * 2.5f * (1+loopIdx);
        pointGridValidator.Initialize(genMetrics, gameBot->m_navBot->GetPosition(), 
            distanceAroundPosition, distanceBetweenPoints, gameBot->m_navBot->GetBotTraverseLogicUserData());

        // Setup the traversal
        Kaim::QueryUtils queryUtils(navDatabase, navDatabase->GetWorkingMemory(), gameBot->m_navBot->GetBotTraverseLogicUserData());
        Kaim::BreadthFirstSearchTraversal< PointGridValidator<LabEngine::GameTraverseLogic> > traversal(queryUtils, pointGridValidator.GetCellBox(), pointGridValidator);
        CHECK(traversal.IsInitialized());

        // Intialize it with a start triangle
        traversal.SetStartTriangle(gameBot->m_navBot->GetNavTrianglePtr().GetRawPtr());
        // Run the traversal
        Kaim::TraversalResult traversalResult = traversal.Search();
        Kaim::KyArray<Kaim::Vec3f>& validPoints = pointGridValidator.GetValidPoints();
        CHECK(traversalResult == Kaim::TraversalResult_DONE && validPoints.GetCount() != 0);


        Kaim::ScopedDisplayList displayList(navWorld);
        displayListIdentifier = displayList.InitUserControlledLifespan("PointGridValidator", "BreadthFirstSearchTraversal", displayListIdentifier);
        for (KyUInt32 i = 0; i < validPoints.GetCount(); ++i)
        {
            displayList.PushPoint(validPoints[i], 
                genMetrics.GetGenerationRadius(), 
                Kaim::VisualShapeColor(Kaim::VisualColor::BlueViolet, Kaim::VisualShapePrimitive_Line));
        }
        displayList.Flush();
        env.Update();


        const KyUInt32 bufferSize = validPoints.GetCount();
        const KyUInt32 factor = 1 + (bufferSize / RAND_MAX);
        const KyUInt32 posIndex = (reproductibleRand.GetRandUInt() * factor) % (bufferSize);
        Kaim::Vec3f dest = validPoints[posIndex];
        gameBot->PushWayPoint(dest);
        env.Update();
        while (gameBot->m_status != LabEngine::GameBot::Arrived)
            env.Update();
        gameBot->ClearRoute();
    }

    navWorld->GetDisplayListManager()->RemoveDisplayList(displayListIdentifier);
    env.Update();
    env.Update();
}


}